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ABSTRACT 


Nonlinear estimators based on the Kalman filter, the extended Kalman filter (EKF) and 
unscented Kalman filter (UKF) are commonly used in practical application. The Kalman 
filter is an optimal estimator for linear systems; the EKF and UKF are sub-optimal 
approximations of the Kalman filter. The EKF uses a first-order Taylor series approxi¬ 
mation to linearize nonlinear models; the UKF uses an approximation of the states’ joint 
probability distribution. Fong measurement intervals exacerbate approximation error in 
each approach, particularly in covariance estimation. EKF and UKF performance under 
varied measurement frequency is studied through two problems, a single dimension falling 
body and simple pendulum. The EKF is shown more sensitive to measurement frequency 
than the UKF in the falling body problem. However, both estimators display insensitivity 
to measurement frequency in the simple pendulum problem. The literature’s lack of 
consensus as to whether the EKF or UKF is the superior nonlinear estimator may be 
explained through covariance approximation error. 

Tools are presented to analyze EKF and UKF measurement frequency sensitivity. 
Covariance is propagated forward using the approximations of the EKF and UKF. Each 
propagated covariance is compared for similarity with a Monte Carlo propagation. The 
similarity of the covariance matrices is shown to predict filter performance. Portions of 
the state trajectory susceptible to EKF divergence are found using the Frobenius norm of 
the Jacobian matrix, limiting the need to consider covariance propagation along the entire 
state trajectory. 

Fong measurement intervals also reveal a commonly overlooked challenge in UKF 
application: sigma point selection methods may produce sigma point vectors that violate 
physical state constraints. Although the UKF can function under this condition over short 
measurement intervals, unexpected failure may occur without consideration of physical 
constraints. A novel constrained UKF, using the scaled unscented transform, is proposed 
to address this issue. 
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CHAPTER 1: 
Introduction 


System state determination remains a challenging engineering problem despite advances in 
measurement technology. Measurements have inherent precision and accuracy uncertainty 
preventing perfect knowledge of the system state. Additionally, each system state may not 
be directly measurable in a given application. Furthermore, in some applications, none 
of the states can be measured directly and the state values must be derived from available 
measurement information. Fundamental measurement uncertainty and unavailability of 
state measurements led to the development of state estimation methods. 

Estimates are commonly generated through use of a system model, a mathematical repre¬ 
sentation of the system. Models, like measurements, also have inherent uncertainty in their 
representation of actual systems. The combination of measurement and system model un¬ 
certainty necessitate a probabilistic vice deterministic representation of the state estimate. 
A stochastic state representation improves understanding of the system state at the expense 
of increased complexity of estimation techniques. 

This dissertation investigates state estimation in instances when not all system states are 
measurable, measurements are subject to sparse availability relative to system states’ po¬ 
tential rate of change, and a nonlinear model describes the system. 

1.1 Motivation 

Navigation, as defined in The New American Practical Navigator, is “the process of plan¬ 
ning, recording, and controlling the movement of a craft or vehicle from one place to an¬ 
other” [1]. Navigation recording is the act of determining the vessel’s position, “a point 
defined by stated or implied coordinates,” at a specified time [1]. The practice of deter¬ 
mining a vessel’s position at sea has significantly changed over the course of history, but 
remains necessary for the safe completion of ocean voyages. Open ocean navigation has 
traditionally involved taking measurements to celestial bodies, when they are in sight, to 
record a fix: a position determined without reference to any former position. When celestial 
bodies are not in sight, the vessel’s position must be estimated, traditionally through dead 
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reckoning. Dead reckoning is the practice of estimating the vessel’s position by plotting 
out the vessel’s direction and distance traveled from the last fix [1], This approach does not 
accurately account for all forces, such as current and wind, acting on the vessel; therefore, 
dead reckoning may not produce an adequate solution for safe navigation over an extended 
time. 

The Global Positioning System (GPS) satellite constellation is commonly the primary 
source for recording position for open ocean navigation today. While the GPS signal is 
almost continuously available for surface vessels, it remains unavailable to submarines op¬ 
erating at depth. The submarine environment requires the navigator to rely on estimating 
the vessel’s position due to the relative sparsity, both in space and time, of available mapped 
features. While dead reckoning can provide a rough estimate of position, inertial sensors, 
first widely employed in the middle of the 20 th century, have demonstrated improved ac¬ 
curacy. Inertial navigation systems measure inertial acceleration and rotational velocity, 
effectively recording all forces acting on a vessel, and integrate these measurements to 
generate an estimated position. 

An exact nonlinear kinematic inertial model exists for a vessel operating near the Earth’s 
surface [2]. However, accurate rotational velocity and linear acceleration measurements 
in the desired reference frame are required to generate an accurate estimated attitude and 
position. Calibration techniques exist that use a known reference to determine system align¬ 
ment errors, sensor bias and scale factors, etc. Although the system alignment is unlikely 
to significantly change while in operation, inertial sensor bias and scale factors values are 
known to drift over time. Integrated imprecise rotational velocity measurements will pro¬ 
duce an inaccurate estimate of vessel attitude over time and, subsequently, an inaccurate 
estimate of position. Unfortunately, inertial sensor bias and scale factors cannot be mea¬ 
sured directly in the absence of a known reference. Therefore, a technique is required to 
accurately estimate sensor bias and scale corrections following initial sensor calibration to 
facilitate accurate position estimation over time. 

The state of practice is to use an extension of the Kalman filter, the extended Kalman 
filter (EKF), to estimate bias and scale factors in inertial navigation systems. The EKF 
enabled practical application of inertial navigation for land vehicles, aircraft, ships, sub¬ 
marines, and spacecraft [2]. However, each application has different requirements for es- 
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timated position accuracy and the maximum time between measurements. The submarine 
navigation problem has particularly challenging requirements, as long duration submerged 
operations require maintaining an accurate position estimate over an extended measure¬ 
ment interval in the absence of navigation fix information. 

This dissertation investigates the hypothesis that an improved parameter mean and covari¬ 
ance estimate will allow for increasing the time between measurements in a nonlinear, 
Kalman filter (KF)-based parameter estimation technique without significantly impacting 
the steady state estimation error. Two well-studied problems, a single dimension falling 
body and a simple pendulum, are considered; each problem employs a nonlinear dynamic 
process model. These problems are analogous to the submarine inertial navigation problem 
and provide insight into the effect measurement interval has on parameter estimation. The 
following literature review provides a general overview of nonlinear estimation techniques. 

1.2 Literature Review 

Estimation theory development was driven primarily by the desire to determine an optimal 
system state estimate from noisy measurements. The measurement signal is available at a 
relatively high rate for a large number of estimation applications. As a result, estimation 
literature does not focus on the effect of sparse temporal measurements. The submarine 
inertial navigation problem that motivates this dissertation is an example of the problem 
type that may benefit from improved understanding of the effect of sparse temporal mea¬ 
surements. 

The previous section presents a number of terms used in the literature without defini¬ 
tion. The following definitions are provided for clarity in better understanding existing 
approaches to nonlinear estimation. Additionally, this section provides a general overview 
of existing approaches to the nonlinear estimation problem along with detailed background 
on KF-based nonlinear estimation techniques. 

A key concept mentioned in the previous section is the state space. Kalman [3] defined this 
concept intuitively as “some quantitative information (a set of numbers, a function, etc.) 
which is the least amount of data one has to know about the past behavior of the system 
in order to predict its future behavior.” This concept is vital if one seeks to generate the 
best possible description of a system from available imperfect measurements. One must at 
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least develop a description of the system that includes the minimum information necessary 
to ensure the validity of future predictions. Reliable prediction is not possible should the 
state space include less than the minimum description. 

Imperfect measurements, y(t ) = x\(t) + w(t), are considered to consist of the signal of 
interest, x\(t), and an additive noise, w(t) [3]. The problem of determining the state 
is therefore fundamentally stochastic since w(t) is not directly observable. Kalman [3] 
additionally notes that observed measurements are often discrete, taking the form of 
y(t) = y(to), y(t \),..., y(tk), where to is the first measurement observed and tk is the 
most recent observed measurement. If tj is the time of interest and t n is the present time, 
the problem will take on different meaning depending upon the relation between t n and 
The term “estimation” collectively refers to smoothing (tj < t n ), filtering (tj = t n ), and 
predicting (tj > t n ) [3]. This dissertation is focused on the filtering problem, but the ideas 
discussed are applicable across the estimation continuum. 

State estimation can be approached from two points of view, either as a batch or recursive 
update. The batch update approach produces an updated state estimate following intervals 
of collecting measurements. As a result, a delay exists between obtaining information 
through individual measurement and incorporation of this information in the state estimate. 
This approach was used by Gauss for the purpose of orbit parameter estimation in the late 
18 th century [4]. 

The recursive approach generates a new update immediately following a measurement [5]. 
The recursive approach is considered in this dissertation as the motivating application ben¬ 
efits from the immediate inclusion of available information. 

Nonlinear estimation refers to estimation in which the descriptions of how the system state 
changes with time and how the measurement relates to the system state are not restricted 
to the linear form. Equations (1.1) and (1.2) show the estimation problem in a linear or 
nonlinear form, respectively. 

x(t ) = F(t)x(t) + D(t)u(t) + G(t)w(t ) 
y(t) = H(t)x(t) + v(t) 


( 1 . 1 ) 
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x(t) = f(x(t),u(t),t ) + d(u(t),t) + g(x(t),t)w(t ) 
y(t) = h(x(t),t,v(t)) 

The estimation problem can also be composed of a combination of linear and nonlinear 
models. 

This dissertation is focused on the problem of estimating the state with infrequently avail¬ 
able measurement information. As noted in Section 1.1, the primary goal of the submarine 
navigation problem is to develop an adequate estimate of the submarine’s position with in¬ 
frequent access to measurement information from external landmarks or beacons. As such, 
sparse temporal measurements in the dissertation title refer to the infrequency of measure¬ 
ment information in time. The definition of sparse is investigated in Chapter 2. 

1.2.1 Problem of Interest 

Mathematical formulation of the problem of interest is shown in Equation (1.3). 

x(t) = f(x(t),u(t),t) + G(t)w(t) ^ 

y(tk) = h(x(t k ),u(tk),v(tk)) 

This problem can be viewed as one of using all available noisy measurement information to 
estimate the system state in a least squares sense. The system dynamics are continuous in 
this problem, but the available measurements are discrete since measurement information 
is not continuously available. 

1.2.2 Bayesian Inference 

Bayesian inference is commonly used to generate a state estimate, x(t) at t - tk■ The 
estimate, x(tk), and noisy measurement, y{tk), are independent assuming that the noise 
corrupting the measurement signal, i’(tk), and the noise associated with the system dynam¬ 
ics, w(t), are independent. Independent distributions have the property shown in Equa¬ 
tion (1.4) [6]. 

Pr(ABC) = Pr(A)Pr(B)Pr(C ) (1.4) 

Therefore, one can use the concept of conditional probability shown in Equation (1.5) 
combined with Equation (1.4) to formulate an estimate of the state conditioned on the 
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noisy measurements, Equation (1.6) [5]. 


Pr(A\B) = 
Pr(B\A ) = 


Pr(AB) 

Pr(B) 

Pr(AB) 

Pr(A) 


Pr(A\B ) = 


Pr(B\A)Pr(A) 

Pr(B) 


Bayes’ theorem, shown in Equation (1.7) [6], summarizes the concept. 


Pr{Aj\B) = 


Pr(B\Aj)Pr(Aj) 

Z Pr(B\A k )Pr{A k ) 

k =1 


(1.5) 

( 1 . 6 ) 


(1.7) 


The prior, or a priori , value of A, P{Aj) occurs immediately prior to the observation of 
event B. The posterior, or a posteriori , value of A, P(A 7 |B) occurs immediately following 
the observation of event B. Kalman [3] notes that the conditional expectation can be used 
analogously, allowing the implementation of a finite dimensional recursive filter described 
in the following section. 


1.2.3 Kalman Filter 

Kalman [3] applied the state transition method to solve the optimal linear estimation prob¬ 
lem for systems with discrete measurements in the form of Equation (1.8) [7]. 

x{t) = F(t)x(t) + G(t)w(t), w(t) ~ N (0 , 2 ( 0 ) 

(l.o) 

h = H k x(t k ) + v k , v k ~ N (0 ,R k ) 

The process noise, w(t), and the measurement noise, v k , are assumed independent and 
Gaussian as shown in Equation (1.8). The assumption of independence allows for ap¬ 
plication of Bayes’ theorem; likewise, independence ensures orthogonality, and therefore 
optimality of the estimate [3]. The estimate is optimal in the minimum mean squared error 
sense for linear process and measurement models with zero mean Gaussian noise. 

The state and covariance estimates are propagated forward to generate a prediction at the 
measurement time by integrating Equation (1.9) [5]. 

x(t) = F(t)(x(t),t) 

P= F(t)P(t) + P(t)F T (t) + G(t)Q(t)G T (t) 
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Kalman [3] propagated Equation (1.9) between t k -1 and t k through the application of a 
state transition matrix, O^-i, to generate a predicted state vector and covariance matrix. 
This predicted estimate is used to generate the optimal stochastic estimate of the state in 
the form of a mean vector and a covariance matrix. The mean vector is produced through 
a linear combination of the predicted state vector and a weighted residual. The residual, 
Res{t k ), or innovation, is the difference between the noisy measurement and the predicted 
measurement as shown in Equation (1.10) [5]. 


Res{t k ) = h ~ H k x k (1.10) 

The resulting estimated state is a linear combination of the a priori estimate and the 
weighted innovation as shown in Equation (1.11) [5]. 

**= + Kkifa ~ H k x~ k ) 

P + k = ( l-K k H k )P~ 

The weight, known as the Kalman gain, K k , is determined, as shown in Equation (1.12) [5]. 

K k = P~H T k (H k P~H T k + R k y 1 (1.12) 

The KF structure requires initialization of a number of factors. These include the pro¬ 
cess noise variance, Qi, the measurement noise variance, Rj, and the initial mean, x(to), 
and covariance, PQo)- 1 The initialization should leverage all information available prior 
to attempting estimation. Information regarding sensor performance, Rj, can be obtained 
via experimentation. Similarly, initial mean and covariance assumptions can be obtained 
through either experimentation or simulation such that the filter is initialized with mean¬ 
ingful information. Selection of Qi is more challenging since this term should account for 
any unknown modeling error. 

Kalman [3] notes that optimality and convergence are only assured if the dynamic system is 
linear. Linearity is necessary to guarantee that the mean and covariance matrix completely 

'The process noise matrix, Q, is a diagonal matrix with terms, Qi, along the main diagonal that represent 
the process noise variance associated with each state, Xj. Each state is assumed independent; therefore, ofF 
diagonal terms are zero. The measurement noise matrix, R , is also a diagonal matrix with terms, Rj, that 
represent the measurement noise variance associated with each measurement, ijj. 
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describe the probability density function of the stochastic state variable [8]. Unfortunately, 
many actual processes are nonlinear dynamic systems and therefore cannot be estimated 
by the KF without making use of an approximation technique. 

The estimated covariance matrix, P(t), will increase appropriately as the time interval be¬ 
tween measurements increases for the linear KF [8]. In other words, the uncertainty associ¬ 
ated with the a priori state estimate grows accurately for long time intervals. The predicted 
uncertainty may become significantly larger than the measurement uncertainty. Therefore, 
the filter will effectively trust the measurement information more than the predicted esti¬ 
mate for sparse temporal measurements; this attribute results in an estimate that closely 
tracks the measurement signal. For this reason, sparse temporal measurements for a linear 
system will reduce the filter’s effectiveness but should not lead to divergence as defined in 
Section 1.2.7. 


1.2.4 Transformation of a Random Variable 

Equations (1.13) and (1.14) detail the transformed mean and covariance, respectively, for a 
time independent transformation, g{x). 


Pg(x) =E[gr(*)] 

S f g(x)p(x)dx i 


.. dx. 


^g(x) —E[(gf(jf) dg(x))(g( x ) dg(x)) ] 

=E [(g(x)g(x) T ] - p g ( x )P T gM 


(1.13) 


(1.14) 


Linear Transformation 

These equations simplify to Equation (1.15) for a linear transformation, g{x) = Ax + b , 
producing the exact transformed mean and covariance matrix using only the mean and 
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covariance prior to transformation. 2 


^g(x) ~ Afl x + b (115) 

^g(x) = AZ x A T 

Analytic solutions, like the linear transformation of a random variable shown in Equa¬ 
tion (1.15), are not universally available for the transformation of random variables. In¬ 
stead, one of the following three techniques are commonly employed for approximation in 
absence of an analytic solution. 


Linearization 

Linearization entails performing a Taylor series expansion of the nonlinear function, g(x), 
shown in Equation (1.16). 


g(x,t) = g(x,t) + 


dg_ 

dx 


(x - x) + 


1 d~g 


2 dx 2 


(x - x)(x - x) T + 


(1.16) 


Once the function, g{x) is linearized, the transformation is performed as shown in Equa¬ 
tion (1.15). 3 First-order expansions (i.e., truncation of Equation (1.16) following the first 
order partial derivative term) are commonly used although higher order Taylor series expan¬ 
sion may be necessary to produce a more accurate approximation. In general, this approach 
provides a poor approximation in regions where the process is highly nonlinear or x sig¬ 
nificantly deviates from the linearization point, x. However, linearization can provide an 
adequate approximation if both conditions are satisfied [9]. 


Monte Carlo 

The Monte Carlo technique applies the law of large numbers to obtain an asymptotically 
close approximation of the probability distribution function following any transformation 

2 This transformation is used for the time independent measurement model of the continuous process- 
discrete measurement time KF. 

3 Higher order Taylor series expansions require corrections to be applied to Equation (1.15). 
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[10]. Equations (1.17) and (1.18) describe the concept. 


dg{x) 


1 

N 


N 

gixi) 
i= 1 


(1.17) 


1 

^g(x) i ~ dg(x))(9( x i) ~ A l g(x)) 

i= 1 

| A' | N 

9(Xi)g(Xi) T - Yj (y {x i^ T g(x) + Vg(x)g(,Xi) T ) (1.18) 

i=l 1=1 

1 N 

+ N - 1 E VgW^gix) 

i= 1 

This method rapidly succumbs to Bellman’s "curse of dimensionality," the exponential 
increase in number of samples, N, required as the dimension of the random variable in¬ 
creases [9]. As a result, processing the number of samples required to obtain an accurate 
approximation of a multidimensional random variable may often require more computation 
time than is available for real time application. 

Unscented Transform 

Another approach, the unscented transform (UT), is “founded on the intuition that it is eas¬ 
ier to approximate a probability distribution than it is to approximate an arbitrary nonlinear 
function or transformation” [11]. In general, estimation problems require the transforma¬ 
tion of ^-dimension random variables. A set of sigma points, S, shown in Equation (1.19), 
consists of vectors, xu an d corresponding weights, W t . that approximate any distribution 
by matching its moments as shown in Equation (1.20). Julier and Uhlmann [11] empha¬ 
size that the approximation should be unbiased (i.e., weights can be positive or negative, 
N 

but Yj Wj = 1, where N is the total number of sigma points used to represent the joint 

;=o 

probability distribution). 

S = {i= 0,1,...,N : XiW} (1.19) 

The UT approach consists of selecting vectors and weights that satisfy the requirements of 
Equation (1.20) where c is a cost function and c are nonlinear constraints that specify the 
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moment matching [11]. 


min c[S,p x (x)] subject to c [S,p x (x)] (1.20) 


For example, the sigma point set shown in Equation (1.22) is the set that minimizes a cost 
function, c, favoring use of the minimum number of vectors, xu while it exactly matches 
the first three moments of a Gaussian distribution; the mean, x, the covariance, P xx , and 
the skew, 0. As such, this sigma point set satisfies the constraints shown in Equation (1.21) 
[ 11 ]. 

ci [S,p x (x)] = YjWiXi-x 

i =0 

c 2 [S,p x (x)] = 2 w i (Xi ~ x) (Xi ~ x) T - P xx (1.21) 

i=0 

c 3 [S,p x (x)] = X W{ (Xi ~ x) 3 

i =0 

In general, distribution approximation accuracy improves with the higher order moments 
being matched. Julier and Uhlmann [12] note that lowest order moments have the greatest 
impact on approximation accuracy. 

Julier et al. [13] detail a symmetric set of 2 n sigma points that accurately capture the first 
two moments, as shown in Equation (1.22) where / = !,... ,//. 


Xi = X+ xj(n + K)P XX Wi = 2(n+K) 

Xi+n — X — V0* + K)P XX Wj+ n = 2(n+K) 


( 1 . 22 ) 


Julier and Uhlmann [12] extend the symmetric sigma point set by incorporating the mean, 
Xo , as shown in Equation (1.23) where i = 1 


XO = X 

Wo 

Xi = x + V(» + «)P XX 

Wi 

Xi+n = X - V(» + K)Pxx 

W l+n 


K 

n+K 

1 

2(n+K) 

1 

2(n+K) 


(1.23) 


This set uses 2/7 + 1 vectors and corresponding weights to accurately capture a /7-dimension 
random variable’s mean, x, and covariance, P xx . The factor, k e S K. provides some freedom 
to more closely match the random variable’s higher order moments. 


The simplex sigma point set was proposed by Julier and Uhlmann [14] to reduce the total 
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number of sigma point vectors used to represent a distribution to n +1 from 2 n +1. Reduced 
computational requirements provided the motivation to define a smaller set of sigma points 
since the UT requires propagation of each sigma point vector. Simplex points are generated 
on the insight that the smallest affine, independent set of points for a given dimension, n, is 
a set of 77 points with an additional point necessary to ensure that the covariance is nonsin¬ 
gular. Julier and Uhlmann [14] note that a triangle, formed by 3 points, forms the largest 
possible set of affinely independent points in a two-dimensional space. A simplex, such 
as a tetrahedron for a three-dimensional space, defines this set when considering higher 
dimensions. 


The penalty for reducing the number of points from 2 n + 1 to 77 + 1 is distortion of the 
represented higher-order moments. Whereas the extended symmetric sigma point set prop¬ 
erly reflected a Gaussian skew of 0, the simplex points do not. The minimal skew simplex 
sigma points address this by selecting the sigma point vectors and weights in a manner that 
minimizes the skew in each dimension [14]. This approach reduces the error in matching 
the skew, the third moment, but does not eliminate it. Weights are selected using the algo¬ 
rithm shown in Equation (1.24) [14], where 0 < Wq < 1. The sigma point vectors, x*- + 


are found by expanding the vector sequence of Equation ( 


sequence is initialized as l Xq = [0] ,x\ = 


yf2W\ 


•x 2 = 


•25)for j 
1 


2 ,... ,77. The vector 


V5WT 


W; 


l-Wo 

2 " 

W\ 

2'~ 2 Wi 


for i = 1 
for i = 2 

for i = 3,... ,77 + 1 


(1.24) 


X 


../+ 1 


4 


-1 


./ 


2 w. 


J J 


0 , 


2 w. 


j j 


for i = 0 


for i = 1 


for 7=y + l 


(1.25) 


The spherical simplex sigma points seek to minimize the hyper-sphere radius that bounds 
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the sigma point vectors to improve numerical stability [15]. The minimal skew simplex 
sigma point set algorithm is altered to achieve this objective. Weights are selected using 
the algorithm shown in Equation (1.26), where 0 < W|q < 1. The sigma point vectors, 
X- are found by expanding the vector sequence of Equation (1.25) for j = 2,...,« [15]. 
The vector sequence is initialized in the same manner as the minimum skew simplex sigma 
points. 

Wi = ^Wo fori = i ^ n+ i (E 26 ) 

77 + 1 

for i = 0 

for z = l,... ,j (1.27) 

for /=y' + l 

The simplex sigma points are not used in this dissertation since the extended symmetric 
set has been shown to produce a better estimate of mean and covariance than the simplex 
sigma point [11]. 

Sigma points are used in the UT to approximate the integrals in Equations (1.13) and 
(1.14) as a weighted sum of the transformed points. This approximation is shown in Equa¬ 
tions (1.28) and (1.29). 

N 

Hg{x) ~^jW (l) g(xi) (1-28) 

i=l 
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N 

^g(x) ~ E W°\g(Xi) - t*g(.x))(g(Xi) - Vg(x)) T 

i= 1 
N 

~ w {l \g(xi)g(Xi) T - g(Xi)^g( X) - g g (x)g(xi) T + 

!=1 (1.29) 

N N v ' 

~ 2 w 0) g(Xi)g(Xif - Yj wi,) (9(Xi)l?g{ x ) + Vg(x)g(Xi) T ) 

i=l i=l 

N 

+ ^ W ( VgU)^g(. v ) 
i=l 

The quality of the UT approximation for nonlinear transformations may depend upon the 
initial sigma point set matching the initial distribution’s higher order moments. 4 As a 
result, the quality of the estimated statistics may be adversely impacted for some nonlinear 
transformations [16]. 

Gustafsson and Hendeby [16] investigated the impact of these transformation techniques on 
filter performance. The specific transformation leads to these three nonlinear transforma¬ 
tion approaches varying in performance [16]. A nonlinear transformation, g(x) = x T x, that 
has an analytic solution is used to demonstrate the effect that the transformation approxima¬ 
tion has on both the mean and covariance. Many other approaches exist to produce sigma 
points. These include methods to match higher order moments such as conjugate unscented 
transformation (CUT) sigma points, presented in [17], and hyper-pseudospectral (HS) 
points, presented in [18], that can include higher order information in the selection of the 
sigma points. Frontera et al. [19] extend Gustafsson and Hendeby [16], demonstrating that 
an UT using HS sigma points that match all moments through the fourth order produces an 
exact transformation unattainable using the first order Taylor series linearization or Monte 
Carlo approaches. 

This dissertation employs the extended symmetric sigma points, detailed in Equation (1.23), 
that exactly match a Gaussian distribution’s first three moments unless otherwise noted. 
The linearization, Monte Carlo, and UT techniques are used throughout the dissertation to 
demonstrate the effect of sparse temporal measurements on nonlinear estimators. 

Application of the UT for a linear transformation, g(x) = Ax + B, produces nearly exact transformed first 
and second moments if the sigma point set exactly matches the pre-transformation first and second moments. 
Any error stems from numerical computation of the summation of the propagated sigma points. 
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1.2.5 Vector Stochastic Process 

Kalman’s [3] insight that “a random function of time may be thought of as the output of a 
dynamic system excited by an independent Gaussian random process,” frames the problem 
of interest. Analogous to the ability to define a state for a deterministic process, the value of 
a vector stochastic process, x(t), at the immediately previous time, tk- 1 , provides as much 
information about x(tk ) as the complete time history x(to),x(t\),... ,x(tu- 1)- 5 This is a 
Markov-1 process, a process in which the current distribution representing the system state 
evolves from the immediately previous distribution [20]. 


The evolution of the state’s distribution, p{X,t\Y tk _ l ), therefore, can be considered as a 
series of transition probability densities that have been shown to satisfy the forward Kol¬ 
mogorov, or Fokker-Planck equation [20]; this partial differential equation is shown in 
(1.30) [21]. 


dpx 

dt 


-Z 


dpFi 1 
dx; 


+ 2 


XZ 


Op (°g°% 

dxjdxj 


(1.30) 


i= 1 ' ' i=l j=1 

p x is the joint probability density function (PDF) of the ^-dimension random variable, x. 


Equation (1.30) is often challenging to solve for practical problems, forcing practitioners to 
apply suboptimal estimators instead. Section 1.2.6 provides a brief description of nonlinear 
estimators that seek to address the potential change in the state’s PDF. Sections 1.2.7 and 
1.2.8 detail KF-based nonlinear estimators that do not attempt to account for the state’s 
PDF changing with respect to time. 


1.2.6 Nonlinear Estimation Techniques 

The following three sections describe methods that seek to account for the change in the 
joint PDF. The joint PDF remains Gaussian in instances when Kalman’s assumptions for 
linearity and Gaussian noise are maintained [20]. As a result, the KF produces an opti¬ 
mal state estimate if these assumptions hold. The EKF and unscented Kalman filter (UKF) 
techniques do not explicitly account for the change in the joint probability density func¬ 
tion. Rather, they operate with an assumption that the approximated conditional mean and 
covariance remain in proximity to the actual mean and covariance to produce a viable sub- 
optimal state estimate. 

5 This has been shown for both continuous and discrete time processes [8], 
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Particle Filter 

The particle filter (PF) employs a Monte Carlo approach to the nonlinear estimation prob¬ 
lem to approximate the multi-dimension integrals required to determine the estimated 
state [22]. A large number of “particles,” state variables randomly drawn from the known 
pre-transformation state distributions, are used with associated weights to determine a 
proportional representation of the post-transformation distribution [22]. While the post¬ 
transformation distribution will not be known exactly to allow for resampling, new parti¬ 
cles are drawn based on a weighted sample in the vicinity of the transformed particles. This 
approach is computationally challenging for applications with a large number of states and 
is subject to a number of implementation challenges [9]. 

Moving Horizon Estimation 

The moving horizon estimation (MHE) was developed with the goal of incorporating state 
constraints within a nonlinear estimation algorithm [23]. Since the conditional probability 
density function, p(xk\yo,i,...,k), is challenging to determine for nonlinear problems, this 
approach instead leverages the conditional probability density function, p(xox...,k\ ?/o,i,...,fc)> 
of the full state trajectory [24]. This entails solving a non-convex constrained optimization 
problem that grows in scope with each time step [24]. The MHE approach is to limit the 
time window for optimization, thereby reducing computational cost [23]. 

Exact Nonlinear Filter 

As noted above, a forward Kolmogorov partial differential equation must be solved to ob¬ 
tain the optimal solution to a nonlinear estimation problem. Exact nonlinear filters seek 
to approximate the solution to the partial differential equation using ordinary differential 
equations. Daum [22] notes that finite dimensional exact filters exist for special cases and 
that a general exact filter has not yet been discovered. 

1.2.7 Extended Kalman Filter 

As noted in Section 1.2.3, the KF assumes a linear process and measurement model in order 
to ensure optimality and convergence. Many applications cannot satisfy this assumption. 
However, approximation techniques allow application of the KF structure to problems of 
the form shown in Equation (1.3). The EKF extends the KF to nonlinear systems by using 
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a Taylor series approximation, introduced in Section 1.2.4, of the process and measurement 
models to linearize the nonlinear functions [5]. 


The EKF employs a first-order Taylor series approximation of the process and measurement 
models. The first-order approximation, obtained by truncating the series after the first- 
order partial derivatives, is used to reduce the algorithm’s computational complexity. Use 
of higher-order Taylor series approximations improves the approximation accuracy at the 
expense of additional computational requirements. The second-order extended Kalman 
filter (EKF2) uses the second-order Taylor series approximation for enhanced accuracy. 


The EKF generates the updated mean estimate, Jc7, by using the best estimate of the mean 
following the previous measurement, jet j, as the initial condition to propagate the nonlin¬ 
ear dynamics, / (. x(t )). The updated covariance estimate, P k , is generated through use of 
a first order Taylor series approximation of the nonlinear process model, f(x(t)). Function 
f(x(t )) is linearized at xt_ 1 using Equation (1.31) [5], and the covariance is propagated 
using Equation (1.32) [5]. 


F(x(t),t) 


df(x(t),t) 
dx(t ) 


x(t)=x(t) 


(1.31) 


x(t) = f(x(t),t) 

Pit) = F(x(t),t)P(t) + P(t)F T (x(t),t) + G{t)Q{t)G T {t) 


This approach effectively allows for the application of the linear Kalman filter structure; 
although x~ k and P k are only approximately the conditional estimated a priori mean and 
covariance, respectively. As discussed in Section 1.2.5, the nonlinear transformation alters 
the distribution over time so that the mean and covariance may not uniquely describe the 
estimated states’ PDF. As a result, the EKF may produce a poor approximation in regions 
where the process is highly nonlinear or the true value of the state significantly deviates 
from the linearization point. However, linearization can provide an adequate approximation 
if both conditions are satisfied [9]. 

The weight, K k , is determined, as shown in Equation (1.33) [5], 

K k = P~ k H T k (x- k ) (H k (x- k )P-H T k (x- k ) + R k )~ l (1.33) 
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with H k , the linearized measurement model, as defined in Equation (1.34) [5]. 


H k (x~ k ) 


dh(x(t k )) 


dx{t k ) 


x(t k )=x k 


(1.34) 


The a priori state estimate, x k , is used in conjunction with the measurement to determine 
the innovation, or residual, Res(t k ), as shown in Equation (1.35) [5], 


Res(t k ) = y k - h k {x k ) (1.35) 

The resulting estimated state is a linear combination of the a priori estimate and the 
weighted innovation, as shown in Equation (1.36). 

K = v + K > ('«-*a*p) 

Pt= (l-K t H t Op)Pi 


The EKF2 uses the same general process to propagate the estimated mean and covariance. 
Second-order Taylor series approximations of the nonlinear models are employed to im¬ 
prove the mean and covariance estimates. The mean is propagated through the nonlinear 
function, and a correction term is applied to remove bias in the a priori mean estimate. 
The correction is determined using the second-order linear function approximation. The 
Kalman gain and a posteriori estimate are also adjusted to improve the estimation accu¬ 
racy [25]. Equation (1.37) [9] shows the change in the a priori estimate, where Tr is the 
matrix trace and is a n x 1 zero vector with 1 in the i th element. 


x(t) = 
P(t) = 


f(x(0,t) + \ Z 0/Tr 


i=l 


d 2 fi 
dx 2 


F(x(t),t)P(t) + P(t)F T (x(t),t ) + G(t)Q(t)G T {t ) 


(1.37) 


Equation (1.38) shows the correction, A k , to the Kalman gain that incorporates the second 
order Taylor series information [9]. 

K k = P~ k H T k (r k ) (H k (x- k )P-H T k (x- k ) + R k + A,)" 1 (1.38) 

Equation (1.39) incorporates the second order Taylor series information in the a posteriori 


18 



estimated mean through the addition of the correction, n k , and the Kalman gain [9]. 


K= *l + K k(yk-h t (r k j)+n k 

The a posteriori covariance is improved through incorporation of the A k term in the 
Kalman gain. The correction terms are detailed in Equation (1.40) [9]. 6 


7T k = 


A k (i,j) = 


\ X 0/Tr 

d 2 hj 

dx 2 

P 

1 = 1 

r 


K . 


d 2 hj(x k J k ) 

p- d 2 hj(x k i k ) 

p~ 

dx 2 

k dx 2 

x k 

x k J 



(1.40) 


As noted in Section 1.2.3, the KF estimate is a weighted combination of information ob¬ 
tained from the model and noisy measurements. As such, the possibility exists that the filter 
can weight the model prediction more than new measurement information. While a desir¬ 
able feature to ignore heavily degraded measurement information, this weighting can lead 
to a condition where the filter rejects all measurement information. Maybeck [8] notes that 
“it is very possible for the filter not to perform as well as it thinks it does. If the computed 
error covariance is inappropriately small, so is the computed gain: the filter weights its in¬ 
ternal system model too heavily and discounts data from the real world too much, leading 
to filter estimates not corresponding to true system performance... a condition called di¬ 
vergence.” The filter, therefore, fails to perform its desired function and becomes a system 
liability. Divergence can be detected through consideration of the residual, as discussed in 
detail in Section 2.3.4. This condition may occur as the result of extended time intervals 
between measurements leading to underestimation of the covariance matrix. 


1.2.8 Unscented Kalman Filter 

The UKF employs the KF structure like the EKF, but rather than linearizing the nonlinear 
model using a Taylor series expansion, it employs the UT, as detailed in Section 1.2.4, 
to indirectly propagate the estimated mean and covariance of the state in between mea¬ 
surements, to generate the predicted measurement, and to determine the Kalman gain. It 
does not approximate the nonlinear process and measurement model, but rather propagates 

6 Simon [9] provides additional details along with the derivation of each correction term. 
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sigma points, Xi where i = 1 ,N, through the actual process and measurement model 
to generate the post-transformation mean and covariance [11]. This approach theoreti¬ 
cally can generate more accurate approximations of the transformed mean and covariance 
in some instances, leading to improved performance as compared to the EKF [11]. 

The UKF represents the a posteriori state estimate, x + k _ v as a set of i = 1,..., N weights, 
wt_ x and vectors, X k -i where N is the total number of points. Sigma points may be 
chosen in a number of ways, as discussed in Section 1.2.4. The a priori state estimate is 
produced, as shown in Equation (1.41) [11]. Process noise, Qk-i, increases the estimated 
covariance [9]. 

H = 7r ^ K- 1 (l> ItL fMO'Xt-i (, V) dt 

i= 1 

p;= b 1 <0 ( 1 . 41 ) 

i=l 

(ftti f&M’Xt-i (, V) dt - x~) T + Q k _1 

Sigma points representing the a priori estimate distribution are selected to match the mean 
and covariance using the process described in Section 1.2.4. The predicted measurement is 
calculated, as shown in Equation (1.42) [9]. 


9k = Jf Z w k {l) h(x k ( '\h) 

1=1 (1.42) 

Py = jf Z w l (,) ( h (xZ - 9k) (h(Xk {l) dk) - 9k) T + Rk 

i= 1 


The Kalman gain is selected using the predicted measurement covariance comprised of the 
propagated predicted measurement covariance and the measurement noise, R k . as shown 
in Equation (1.42). The cross covariance between x k and y k is calculated, as shown in 
Equation (1.43) [9]. 


xy 


N 


= N 2 »>] 
1=1 


(0 


(ftti f(x + k- 1 {i) dk) dt - X k ) (h(x k (i) ,tk ) - 9k) 1 


(1.43) 
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The Kalman gain is selected, as shown in Equation (1.44) [11]. 


K k = P xy P~ l (1.44) 

The assumption that the measurement noise is uncorrelated with both the a priori estimate 
and estimate error is central to the derivation of the UKF Kalman gain. Although Equa¬ 
tion (1.44) appears different from the EKF Kalman gain shown in Equation (1.33), authors 
of [16] and [26] have shown their equivalence. The UKF measurement update takes the 
form shown in Equation (1.45) [11]. 

K = *l + K k (y k -y k ) 

P t = p k - K kPyK T k 


1.2.9 EKF and UKF Performance Comparison 

The literature is reviewed for comparison of the EKF and UKF performance in various 
applications. The following examples of EKF and UKF performance comparisons are pro¬ 
vided to demonstrate that despite 20 years having passed since the introduction of the UKF, 
a clear consensus as to the relative performance of the two approaches remains elusive de¬ 
spite the UT’s theoretical second-order accuracy being superior to the first-order Taylor 
series approximation. 

Kurt-Yavuz and Yavuz [27] compare an EKF and UKF application to the simultaneous 
localization and mapping problem. This work concluded that the UKF outperformed the 
EKF on average in this application. Ristic et al. [28] came to a similar conclusion for the 
ballistic re-entry tracking problem. Crassidis [29] presents a comparison of an EKF and 
UKF in an aircraft inertial navigation system application. GPS measurements are received 
at a 1 Hz rate. This comparison reveals that the UKF outperformed the EKF in instances 
with large initialization errors, but similar performance is noted in instances with small 
initialization errors. 

Giannitrapani et al. [30] compared an EKF and UKF, estimating spacecraft position using 
angles-only measurements from the spacecraft to the Earth and Moon while traveling along 
a Earth-to-Moon transfer orbit and a geostationary orbit-raising trajectory. Measurements 
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are taken each hour and the error for both estimators is the same order of magnitude. The 
EKF and UKF are implemented with different process noises; a larger process noise is used 
with the EKF to tune the filter for improved consistency. Giannitrapani et al. [30] do not 
consider varying the measurement frequency, choosing to vary the relative angle between 
the objects. They conclude that the acUKF demonstrated superior “average localization 
error and consistency of the estimates.” 

Allotta et al. [31] discuss the application of an EKF and UKF for estimating the position 
of an autonomous underwater vehicle. This work centers on a simulation using previously 
collected sensor information, specifically, a Doppler velocity log to measure speed through 
water, a pressure transducer to provide depth measurements and an inertial measuring unit. 
Both filters are initialized with the GPS initial position and the filters are propagated for¬ 
ward in time until another GPS position is received. Filter performance is determined by 
comparing the EKF and UKF estimated positions with the GPS termination point, respec¬ 
tively. Allotta et al. [31] conclude that the UKF outperformed the EKF on the basis of 
termination position error. The analysis presented does not investigate the effect of sparse 
temporal measurements, as the sensor data obtained is not KF measurement data (i.e., GPS 
position). Rather, the sensor data are actually inputs into the process model. Their work, 
however, demonstrates that the EKF significantly underestimates the estimated position 
covariance compared with the UKF. 

Rhudy et al. [32] note the lack of a clear consensus in the literature comparing EKF and 
UKF performance and cite additional examples of performance discrepancies in a wide 
variety of applications. Rhudy et al. conclude through studying three analytic nonlinear 
transformations that the UKF provides equal or better performance compared with the EKF 
in determining the mean, but the covariance determination performance varies. Their ap¬ 
proach, however, does not consider the effect of longer propagation intervals on covariance 
transformation. 

One case of direct comparison between the EKF and UKF at varied measurement fre¬ 
quencies is found in the literature. Faviola [33] compared the performance of these two 
approaches in the case of a virtual reality head and hand tracking example. The frequen¬ 
cies considered, 25 Hz, 80 Hz, and 215 Hz, in [33] are relatively fast compared to the 
system time constant during this experiment. As a result, a significant difference between 
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the EKF and UKF estimate does not exist. Faviola [33] notes that “if the sampling rate 
is sufficiently high, ... the error in linearization minimal.” This result and interpretation 
suggest that performance of the EKF and UKF should be nearly identical in any system if 
the measurement rate frequency is sufficiently fast. 

Both estimators in [33] are initialized identically to allow for a comparison in performance. 
Of interest, process noise, Qk, is included in both cases and selected in the same manner. 
Inclusion of process noise covariance establishes the minimum level of confidence that the 
filter will have in the a priori estimate and may mask differences between the approxima¬ 
tion approaches. Faviola [33] notes that in the 25 Hz case neither estimator significantly 
outperforms the raw measurements statistics suggesting that this measurement frequency 
is not sufficiently fast enough to merit filtering. However, the inclusion of process noise 
could explain the failure of either filter to outperform the raw measurements along with the 
EKF outperforming the UKF. 

1.3 Contributions 

The literature does not contain detailed study of KF-based nonlinear estimator performance 
with sparse temporal measurements. Therefore, this dissertation investigates the effect of 
measurement frequency on the EKF and UKF for nonlinear parameter estimation. This 
approach was chosen in lieu of seeking to either directly solve the forward Kolmogorov 
partial differential equation or find its approximate solution because the state of practice 
entails using a version of the KF in operational systems. 

KF-based nonlinear estimators employ feedback to correct for approximation errors; feed¬ 
back provides robust filter performance in most conditions while potentially masking un¬ 
derlying aspects that could lead to unexpected poor performance. This dissertation high¬ 
lights that comparison between KF-based nonlinear estimators is challenged by the rel¬ 
atively short measurement time intervals found in common application. Differences in 
estimator performance are explained through study of the filters using longer measurement 
intervals. 

Chapter 2 employs a single-dimension falling body problem to demonstrate that the EKF 
and UKF may respond differently when subjected to sparse temporal measurements. The 
propagation of covariance between measurements is shown to cause the difference in esti- 
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mator performance. Additionally, longer measurement intervals highlight that propagation 
of an individual sigma point can significantly impact the quality of the UT. Specifically, 
physical constraint violations may lead to a single sigma point propagation producing un¬ 
realistic results that dominate the results from of the all other sigma points. This aspect 
is overlooked in the literature since the error magnitude is often small over short propaga¬ 
tion intervals. A novel approach to implement a constrained UKF is presented to facilitate 
study of the UKF response under longer measurement intervals. This approach leverages 
the Julier’s scaled UT [34] in a new way that ensures underlying sigma point accurately rep¬ 
resent the estimated state while respecting parameter constraints. The measurement update 
state of the UKF is altered if necessary to ensure that the state estimate respects constraints. 

Chapter 3 employs a simple pendulum problem to provide experimental validation of the 
measurement frequency effect shown through simulation in Chapter 2. The pendulum prob¬ 
lem also demonstrates that the effect of sparse temporal measurements is problem depen¬ 
dent. Additionally, this problem illustrates that the UT does not produce a more accurate 
approximation of a propagated random variable in all instances. 

Chapter 4 proposes two complementary techniques for predicting if the propagated covari¬ 
ance will be significantly affected by sparse temporal measurements in a given application. 
The process model Jacobian is analyzed to locate portions of the state trajectory that may 
result in significant errors impacting estimator performance. Covariance propagation is 
analyzed through these regions to determine if filter performance may be impacted by ap¬ 
proximation error. This approach facilitates characterization of a problem in advance of 
filter application, providing the designer with a means of predicting application challenges 
resulting from measurement frequency. 

Conclusions are provided in Chapter 5 along with future work necessary to advance KF- 
based nonlinear estimation in parameter estimation applications with long measurement 
intervals. 


24 



CHAPTER 2: 
Falling Body Problem 


A single-dimension falling body problem, described in detail in Section 2.1, is used to 
demonstrate the effect of sparse temporal measurements on KF-based nonlinear estimators. 
The details of the simulation are presented in Section 2.2. Section 2.3 simulation results 
uncover differences in estimator performance as a result of the measurement frequency. 

Section 2.4 provides background on constrained UKF. Section 2.5 proposes the scaled un¬ 
scented Kalman filter (UKF-S) as a method of accounting for parameters that are inequality 
constrained. This method, based on the traditional UKF, uses scaling of the sigma points 
and Kalman gain, as necessary, to respect parameter constraints. The falling body problem 
is used to demonstrate UKF-S application. 

2.1 Problem Description 

Figure 2.1 depicts an object falling in a single-dimension through an atmosphere while 
being monitored periodically by a single radar. Equation (2.1) details the states and param¬ 
eters used in the model. 



Figure 2.1. Falling body problem. Adapted from [25]. 
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x\ = Altitude, ft x 2 - Velocity, ft/s 

JC 3 = Ballistic Coefficient, 1/ft x\ = Gravitational Acceleration, ft/s 2 

Athans et al. [25] first proposed using this classic problem in a 1968 paper to compare the 
EKF and EKF2. Athans et al. sought to use a single radar to track multiple objects by in¬ 
creasing interpulse radar transmission periods. Authors of [35], [36], [37] have since used 
this problem to compare novel estimation techniques with the EKF. An additional param¬ 
eter, gravitational acceleration, that is usually not considered in the literature is estimated, 
as shown in Equation (2.1), to increase the dimension of the state space. 

Despite using this problem to evaluate nonlinear filter performance, the effect of measure¬ 
ment update frequency was not explicitly considered in [20], [25], [35]-[37]. Each author 
assumes measurements at a 1 Hz rate. 

Equation (2.2) describes f (x(t)) how each state changes in time while a body falls toward 
a planet’s surface through an atmosphere. 


it 


~X 2 



0 

x 2 

= /U( 0 ) = 

-exp(-y.ri)x 2 X3 + X 4 

+ 

w 2 - 

- W, 0 2 ) 

X3 
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W3 - 

(0,2 3 ) 

X4 


0 



0 


The process noise, Wj(t), represents the uncertainty associated with the modeled dynamics. 
As noted in [25] and Section 1.2.7, the process noise is chosen to be Gaussian distributed, 
~ N(0,Qj(t)). Velocity and acceleration in Athans et al. [25] are positive when the body 
is traveling toward the ground and negative when traveling upward in the atmosphere, y is 
a parameter defining the exponential increase in the density of the atmosphere at altitudes 
close to the surface of the planet. This example assumes a number representative of Earth’s 
atmosphere, y = 5 x 10 -5 1/ft [25]. 

The ballistic coefficient and gravitational acceleration are constant parameters, iy = iq = 
0, with no anticipated dynamics. The state space describing the system dynamics is aug¬ 
mented with parameters to improve model fidelity as is the practice for inertial navigation 
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systems. The ballistic coefficient, * 3 , is always positive, as defined in Equation (2.3) [25]. 


JC 3 = C ^ Po : Ballistic Coefficient (1/ft), where 

C[) : Drag Coefficient, dimensionless, > 0 by definition 
A : Surface Area, ft 2 

po : Atmospheric Density at surface, lb/ft 3 
m : Mass, lb 


Gravitational acceleration in Athans et al. [25] is positive and acts to attract the body to¬ 
wards the ground since the planet is assumed to be of significantly greater mass than the 
falling body. The gravitational acceleration is effectively constant over the studied altitude 
range. Equation (2.4) details the slant range radar measurement equation, /p (x(tjt)), used 
to relate the state to the radar distance measurement [25]. 

r(tk) = \j A7 2 + (x\(lk) ~ H) 2 , where M: radar horizontal offset 

(2.4) 

H : radar altitude 


Measurements are assumed to occur at specified discrete intervals, tk- The measurement 
model is nonlinear. 


2.2 Simulation Details 

The system of equations shown in Equation (2.2) is propagated in time to generate a truth 
model using MATLAB’s odeA5 function. The model is considered to perfectly represent 
the system; therefore, the process noise, «;,■(?) = 0, is nonexistent for each state. Assumed 
actual initial conditions are x\ = 300,000 ft; X 2 = 20,000 ft/s; *3 = 1 x 10 -3 1/ft; and 
X 4 = 32.17405 ft/s 2 . Figure 2.2 shows the truth model used for this example throughout 
the dissertation. The nonlinear effect is most prominent in the area highlighted by the blue 
rectangle between t - 6 s and t = 22 s. The falling body’s rapid exponential deceleration 
during this time as it interacts with an increasingly dense atmosphere causes this effect. 

As shown in Equation (2.5), the radar range measurements are assumed noisy to accurately 
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Figure 2.2. Truth generated by propagating initial conditions for the falling 
body problem: (a) depicts altitude (*i) and (b) velocity (* 2 ). 


reflect the sensor’s inherent measurement uncertainty [25]. 

y(x(tk)) = r(ti) + v{tk), where v(tk)'- measurement noise, ~ N(0,cr 2 ) (2.5) 

Results presented assume the radar is on the surface (H = 0), providing noisy measure¬ 
ments with ~ N (0 ft, 1 x 10 4 ft 2 ) distribution. Figure 2.3a highlights the nonlinear rela¬ 
tionship between altitude and radar range. A 100 trial Monte Carlo study based on the 
trajectory depicted in Figure 2.2 is conducted to determine the filter performance. Each 
trial uses a unique measurement signal corrupted by independent noise. Measurements are 
generated at a 100 Hz rate and sampled as indicated to test filter performance with longer 
measurement intervals. Each filter is tested using the same 100 trials to allow performance 
comparison. Figure 2.3b noisy measurements are a representative example of the measure¬ 
ment error used within each individual trial for filter comparison throughout this section. 


The EKF, EKF2, and UKF are initialized identically in mean and covariance to facilitate 
comparison with respect to the measurement update rate. Results provided in Section 2.3 
used the following initialization parameters: altitude, *i(0) = 300,000 ft, velocity * 2 ( 0 ) = 
20,000 ft/s, ballistic coefficient, * 3 ( 0 ) = 3x 10 -5 1/ft, and gravitational acceleration, 
* 4 ( 0 ) = 32.17405 ft/s 2 . The ballistic coefficient, * 3 ( 0 ), is not initialized with the value 
used to generate the truth since this parameter is likely not well known prior to estimation. 
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Figure 2.3. Radar slant range measurements: (a) depicts calculated true 
radar slant range and (b) representative radar measurement error used in 
simulation. 


The variances associated with each state are P Xl ( 0) = 1 x 10 6 ft 2 , P X2 (0) = 4 x 10 6 (ft/s) 2 , 
P X3 = 1 x 10 -4 (1/ft) 2 , and P XA = 1 x 10 -4 (ft/s 2 ) 2 . 7 The initial level of uncertainty is set 
to reflect the likely knowledge of each state when estimation commences. 

A fixed step, Runge-Kutta fourth-order solver is used to propagate the process model be¬ 
tween measurements. The measurement interval determines the total propagation time. 
The maximum integration step size is limited to 0.01 s to ensure that the integration method 
does not contribute to degrading the estimate [25]. All filters assume a measurement noise 
identical to that used to generate the measurements, ~ N(Q ft, 1 x 10 4 ft 2 ). Although pro¬ 
cess noise could be introduced to “tune” the EKF [20], it is not considered in this case since 
the model is known to accurately represent the process. 


2.3 Simulation Results 

MATLAB is used to conduct a 100 trial Monte Carlo study to compare the performance 
of the EKF, EKF2, and UKF for varied measurement frequencies. Figure 2.4 shows the 
average absolute error for each state with the measurement frequency at a 1 Hz rate as 
is common in the literature. These results are consistent with those obtained by Athans 
et al. [25]. The EKF2 and UKF outperform the EKF in terms of average absolute steady 

7 These initial conditions are similar to those used by Athans et al. [25]. 
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state altitude and velocity a posteriori estimation error. The EKF2 and UKF exhibit similar 
performance in all states. 
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Figure 2.4. Comparison of EKF, EKF2, UKF estimation performance at 1Hz 
measurements, 100 trial average: (a) depicts average absolute altitude, jci, 
error, (b) average absolute velocity, X 2 , error, (c) average absolute ballistic 
coefficient, x?,, error and (d) average absolute gravitational acceleration, X 4 , 
error. 


Average absolute error results generated using a lower 0.5Hz measurement frequency are 
presented in Figure 2.5. At this measurement frequency, the UKF demonstrates superior 
steady-state performance compared to the EKF and EKF2. Figure 2.5a most clearly high¬ 
lights the performance difference. 

The same simulation is run using denser measurements to determine filter performance 
with shorter intervals between measurements. Representative results are presented in Fig¬ 
ure 2.6 at the 2Hz measurement rate. All three filters produce nearly identical results at 
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Figure 2.5. Comparison of EKF, EKF2, UKF estimation performance at 
0.5Hz measurements, 100 trial average: (a) depicts average absolute alti¬ 
tude, x\, error, (b) average absolute velocity, X 2 , error, (c) average absolute 
ballistic coefficient, * 3 , error and (d) average absolute gravitational acceler¬ 
ation, * 4 , error. 


a measurement frequency of 5Hz. These results suggest that when the measurement fre¬ 
quency is sufficiently fast , the different EKF and UKF approximation techniques do not 
impact estimator performance. 


31 




















120 


_ 100 

o 80 
lij 

60 

^s 

0 

2 40 
< 20 

0 







-EKF 

-EKF2 


\ 





is/ 

% 





; & 





10 


20 30 40 

Time(s) 


50 


60 


(a) 



Time(s) 


(b) 



(c) 



(d) 


Figure 2.6. Comparison of EKF, EKF2, UKF estimation performance at 2Hz 
measurements, 100 trial average: (a) depicts average absolute altitude, x\, 
error, (b) average absolute velocity, X 2 , error, (c) average absolute ballistic 
coefficient, xj, error and (d) average absolute gravitational acceleration, X 4 , 
error. 


2.3.1 Measurement Frequency Greater Than or Equal 0.5 Hz 

More detailed results from this study are presented in Figures 2.7 - 2.9 for the EKF, EKF2, 
and UKF, respectively. These plots highlight the effect of measurement frequency on 
estimator performance. The three-dimensional and two-dimensional views show the same 
100 trial average absolute altitude estimation error. The plots are produced by calculating 
the average absolute error using measurement frequencies of 0.5 Hz - 1 Hz at 0.1 Hz 
intervals and 2 Hz and interpolating. Both views are presented and the axes and color 
bars are identical to facilitate comparison between the three filters. For this particular 
problem, measurement rates in excess of 1.5 Hz produce negligible differences between 
these nonlinear estimation techniques. 
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Figure 2.7. EKF, average absolute altitude, x\, error, 100 trial average, with 
measurement interval of 0.5 s and 2 s (measurement frequency of 2 Hz and 
0.5 Hz): (a) depicts time versus measurement interval versus error and (b) 
time versus measurement interval. 


Comparison of the three KF-based nonlinear estimators reveals that the sensitivity to mea¬ 
surement frequency is reduced through use of a “more accurate” [11] approximation tech- 
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Figure 2.8. EKF2, average absolute altitude, x\, error, 100 trial average, 
with measurement interval of 0.5 s and 2 s (measurement frequency of 2 Hz 
and 0.5 Hz): (a) depicts time versus measurement interval versus error and 
(b) time versus measurement interval. 

nique. However, both the EKF and EKF2 reject information available in the measurements 
t = 15 s as a result of underestimated state covariance. 
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Figure 2.9. UKF, average absolute altitude, x\, error, 100 trial average, with 
measurement interval of 0.5 s and 2 s (measurement frequency of 2 Hz and 
0.5 Hz): (a) depicts time versus measurement interval versus error and (b) 
time versus measurement interval. 
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2.3.2 A Priori Covariance Estimation 

KF-based nonlinear estimators fundamentally differ in the estimation of the a priori covari¬ 
ance. As discussed in Section 1.2.3, an accurately propagated covariance is a fundamental 
assumption in the KF optimal gain selection. The estimators will improperly exclude infor¬ 
mation available in the measurement if the a priori covariance is underestimated. The effect 
of measurement frequency on the a priori covariance estimates can be seen by comparing 
Figures 2.10 and 2.11. 
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Figure 2.10. Comparison of estimated standard deviation, 100 trial average, 
2 Hz measurements: (a) depicts average estimated altitude (xi) standard 
deviation, (b) average estimated velocity (* 2 ) standard deviation, (c) aver¬ 
age estimated ballistic coefficient (* 3 ) standard deviation and (d) average 
estimated gravitational acceleration ( 24 ) standard deviation. 


The time between t = 6 s and t = 22 s, the region where the nonlinear effect is most 
prominent as discussed in Section 2.2 is shown in these figures. The standard deviation 
associated with each state as estimated by the EKF and EKF2 is smaller than the UKF 
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Figure 2.11. Comparison of estimated standard deviation, 100 trial average, 
0.5 Hz measurements: (a) depicts average estimated altitude (.vi) standard 
deviation, (b) average estimated velocity ( X 2 ) standard deviation, (c) aver¬ 
age estimated ballistic coefficient (* 3 ) standard deviation and (d) average 
estimated gravitational acceleration (* 4 ) standard deviation. 


estimate at 0.5 Hz. The underestimated covariance results in the filter lowering the weight 
placed upon the innovation; the EKF weights the prediction more than the measurement 
as a result of underestimating the propagated uncertainty. Julier and Uhlmann [12] note 
that a consistent covariance estimate, as defined in Equation (2.6), is necessary for filter 
convergence. 

P k - E[(x k - x k )(x k - Xk) T ] > 0 (2.6) 

The actual a priori covariance in Equation (2.6) results from E [(x k - Xk)(x k - x k ) T ], where 
xk is the actual vice estimated state and x k is the actual mean of the state. An optimal 
estimator requires P £ = E[(x k - Xk)(x k - Xk) T ]- 

At higher frequencies, as shown in Figure 2.10, the EKF, EKF2, and UKF generate nearly 
identical a priori standard deviation estimates for each state. This results in nearly identical 
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estimation performance at the 2 Hz measurement frequency seen in Figure 2.6. 

2.3.3 Kalman Gain 

Information obtained through each measurement is incorporated into the propagated state 
estimate by weighting the innovation with the Kalman gain, as shown in Equation (1.11). 
The Kalman gain is calculated for the EKF and the EKF2, as shown in Equations (1.33) 
and (1.38), respectively. The UKF Kalman gain is calculated, as shown in Equation (1.44). 
The Kalman gain should become small as the filter converges in steady state since there 
will be limited new information available in each innovation. 

The effect of measurement frequency on the average Kalman gain associated with the alti¬ 
tude, jti, and velocity, xi state estimates at 2 Hz, 1 Hz, and 0.5 Hz is shown for the EKF, 
EKF2, and UKF in Figure 2.12, Figure 2.13, and Figure 2.14, respectively. Ballistic coef¬ 
ficient, X 3 , and gravitational acceleration, X 4 , are not shown due to the very small Kalman 
gain magnitude differences. 




(a) (b) 

Figure 2.12. Comparison of EKF Kalman gain, 100 trial average, at varied 
measurement frequency: (a) depicts average altitude (jci) Kalman gain and 
(b) average velocity ( X 2 ) Kalman gain. 


The estimated a priori covariance, P^, the uncertainty associated with the state estimate, is 
approximated using different techniques in each approach, as discussed in Section 1.2. This 
can result in a covariance matrix that does not reflect the true uncertainty in the state, as seen 
in Figure 2.11. The Kalman gain is calculated for the EKF, as shown in Equation (2.7), for 
this particular problem. P~ kj . is the a priori estimated covariance component with indices i 
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Figure 2.13. Comparison of EKF2 Kalman gain, 100 trial average, at varied 
measurement frequency: (a) depicts average altitude (vq) Kalman gain and 
(b) average velocity (aq) Kalman gain. 
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Figure 2.14. Comparison of UKF Kalman gain, 100 trial average, at varied 
measurement frequency: (a) depicts average altitude (^i) Kalman gain and 
(b) average velocity (aq) Kalman gain. 


and j. Hkjj is the measurement Jacobian at time, k, with indices i and j. 


K k 


P k, n Hk ’ n 

P k,21 H kM J _1_ 

P kJ l Hk ’ n \ Hk ’ uP k,U Hk ' ] + Rk 

P kM H k.n_ 


(2.7) 


The value of Hk,n will vary depending on the estimated altitude, as shown in Figure 2.15. 
The value of the measurement Jacobian will scale each state’s Kalman gain, as shown 
in Equation (2.8). The measurement noise, Rk, will have a greater relative effect on the 
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Figure 2.15. Measurement Jacobian (Hu) value versus altitude, falling body 
problem 


Kalman gain in comparison with the estimated covariance as altitude decreases. 




( 2 . 8 ) 


Comparison between the EKF, EKF2 and UKF 100 trial average Kalman gain magnitude 
at 2 Hz measurement frequency reveals a similar weighting profile between all approaches, 
shown in Figure 2.16a. Comparison of the average Kalman gain magnitude at 0.5 Hz 
measurement frequency reveals a significantly larger weighting for the UKF, as shown 
in Figure 2.16c. Figure 2.16b is provided for comparison as the common measurement 
frequency used in the literature. This difference in average magnitude of the Kalman gain 
in Figure 2.16c results in the UKF incorporating more information from measurements 
compared to the EKF and EKF2. The plots provided in this section highlight that the 
EKF and EKF2 are converging, on average, earlier in time than the UKF. Unfortunately, 
this convergence prevents incorporation of valuable information in the measurements. The 
Kalman gain difference results in the UKF’s reduced steady state convergence error in 
comparison to the EKF and EKF2. 
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Average Kalman Gain Magnitude 



Time(s) 


(a) 



Time(s) 


(b) 



Time(s) 


(c) 

Figure 2.16. Comparison of EKF, EKF2 and UKF average Kalman gain 
magnitude, 100 trial average, at varied measurement frequencies: (a) de¬ 
picts average Kalman gain magnitude, 2 Hz measurements, (b) average 
Kalman gain magnitude, 1 Hz measurements and (c) average Kalman gain 
magnitude, 0.5 Hz measurements. 

2.3.4 Innovations 

Additional insight is obtained by considering the innovation, or residuals, Res(t k ), as 
shown in Equation (2.9). 

Res{t k ) = h ~ 9k 


= y k -h(x tk ) ( 2 -9) 

= (r(t k ) + v{t k )) - VM 2 + (jci(f *)-//) 2 
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The actual radar range at any specified time is a function of the actual state value, as shown 
in Equation (2.4). The estimated state, x\(tk), is the actual value of the state, x\ (f*), plus 
the error between it and the actual value, x,\(tk) = x\ ( 4 ) - x\ ( 4 ). Therefore, the residual 
can be rewritten, as shown in Equation (2.10). 

Res(t k ) = ( yjM 2 + x\{tk ) 2 + v(t k )) - yfM 2 + (x\(t k ) + x\{t k )) 2 

= v(tk) + ( ^M 2 + xi(t k ) 2 - aJm 2 + x\(t k ) 2 + 2 xi(t k )xi(t k ) + x\(tk) 2 ) 

(2.10) 

The residual will become v(tk) as x\ —> 0. Maybeck [8] notes that the residual sequence 
has been shown to be white, and Gaussian for the linear filter. The residual sequence can be 
monitored to determine if the filter is performing properly. Maybeck also notes that a mov¬ 
ing window is the most appropriate way of considering filter performance, as a single large 
residual does not indicate filter failure. When the filter is performing correctly in steady 
state, the innovations over time should reflect the noise associated with the measurement. 

This approach seeks to determine if the innovation sequence demonstrates a bias or a pro¬ 
cess strength increase. In other words, the innovations are expected to exhibit a noise 
profile similar to that shown in Figure 2.3b. Figure 2.17 shows the innovations generated 
when using the same representative single trial by the EKF, EKF2 and UKF at two dif¬ 
ferent frequencies. As the measurement interval is lengthened, the EKF, EKF2, and UKF 
innovations appear representative of the measurement noise for this trial. 

The 100 trial average innovation for each filter at a given measurement frequency charac¬ 
terizes the filter performance. The average innovation, Res, is shown in Equation (2.11) 
and is the expectation of the residual, Equation (2.10). 

- i N 

Res - jf X R es i(h ) f° r k = 0, At,2At,... ,tf 

i= 1 

Resitk) = E [v(t k ) + ( yjM 2 + x\(t k ) 2 - ^M 2 + x\(t k ) 2 + 2xi(t k )xi(t k ) + x\(t k ) 2 )\ 

= E [ \jM 2 + xi(t k ) 2 ] - E [ yjlVL 2 + xi (t k ) 2 + 2x\(tk)x\(tk) + xi(U-) 2 ] 

(2.11) 

The average residual should disappear as x,\ —> 0, as shown in Figure 2.18a. All filters 
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Figure 2.17. Comparison of EKF, EKF2 and UKF innovations from a single 
trial: (a) depicts 2 Hz measurements and (b) 0.5 Hz measurements. 


display a process model error, as seen in Figure 2.18, during the first 10 seconds since the 
filter has not yet corrected the error from the ballistic coefficient, * 3 , initialization. 

Non-random filter error will appear as a deviation from 0, as visible in Figure 2.18b most 
clearly for the EKF. The effect of measurement frequency of filter performance is clearly 
seen by comparing the average innovations shown in Figures 2.18a and 2.18c that have the 
same axes. 
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(a) (b) 



Figure 2.18. Comparison of EKF, EKF2 and UKF 100 trial average innova¬ 
tions: (a) depicts 2 Hz measurements, (b) 0.5 Hz measurements and (c) an 
expanded view at 0.5 Hz measurements. 

2.3.5 Measurement Frequency Less Than 0.5 Hz 

Further lowering measurement frequency produces the unexpected results shown in Fig¬ 
ure 2.19 —failed UKF execution occurring in conjunction with the partially successful 
EKF execution. Figure 2.19 reveals that the three filters are unable to converge to a stable 
steady-state average absolute error. Although the UKF shows better performance than the 
EKF2 at the 0.5 Hz measurement frequency, as shown in Figure 2.5, the UKF is unable to 
complete any of the 100 trials at the 0.3 Hz measurement rate. The EKF fails to execute on 
75% of the trials and does not converge when it executes. The EKF2 diverges in all cases, 
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but is able to run for the full simulation duration. 
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Figure 2.19. Comparison of EKF, EKF2, UKF estimation performance at 
0.3Hz measurement frequency, 100 trial average: (a) depicts average abso¬ 
lute altitude (xi) error, (b) average absolute velocity (.* 2 ) error, (c) average 
absolute ballistic coefficient (X 3 ) error and (d) average absolute gravitational 
acceleration (* 4 ) error. 


This unanticipated result requires a more detailed investigation of each UKF trial to find 
the cause for the performance failures. Close inspection reveals velocity, X 2 , approaches 
positive infinity in each trial. The object accelerates toward the surface at an increasing 
rate rather than decelerating as anticipated. This condition is not physically possible given 
the problem construct shown in Figure 2.1. The acceleration, X 2 , can increase downward 
only if the ballistic coefficient, X3, is negative, as shown in Equation (2.2). However, Equa¬ 
tion (2.3) demonstrates that X 3 must be positive by definition. This constraint violation 
suggests a need to consider the individual sigma point vectors that are propagated through 
the dynamic model between discrete measurements. 
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Each state has physical limits in this particular problem. Altitude, like the ballistic coeffi¬ 
cient, must remain positive since the resistance to the body’s motion would be significantly 
higher below the surface than through the modeled atmosphere. Velocity and the gravita¬ 
tional acceleration both have minimum and maximum physical limits. Velocity magnitude 
is limited by the physical characteristics of the body, as high speed may lead to disinte¬ 
gration. Similarly, the gravitational acceleration is limited in magnitude. Close inspection 
of the initial sigma points at t = 0, as shown in Table 2.1, reveals that in one instance the 
ballistic coefficient, or (3. *3 has a negative value. These sigma points are generated using 
the extended symmetric approach detailed in Section 1.2.4, assuming filter initialization 
parameters of Section 2.2. 


Table 2.1. Falling body example of Section 2.2 initial 2/7 + 1 sigma points. 


Sigma Points (2n+l) 

1 

2 

3 

4 

5 

6 

7 

8 

9 

Weight 

0 

0.125 

0.125 

0.125 

0.125 

0.125 

0.125 

0.125 

0.125 

Altitude, xi (ft) 

300000 

302000 

300000 

300000 

300000 

298000 

300000 

300000 

300000 

Velocity, X 2 (ft/s) 

20000 

20000 

24000 

20000 

20000 

20000 

16000 

20000 

20000 

/?, * 3 (1/ft) 

0.01 

0.01 

0.01 

0.03 

0.01 

0.01 

0.01 

-0.01 

0.01 

g, r 4 (ft/s 2 ) 

32.17405 

32.17405 

32.17405 

32.17405 

32.19405 

32.17405 

32.17405 

32.17405 

32.15405 


Figure 2.20 shows propagation divergence of a single initial sigma point, point number 8 
from Table 2.1, violating the state constraints. The divergence occurs after a propagation 
time longer than 8 seconds, a period of time greatly in excess of the 1 second propagation 
that occurs for the literature standard 1Hz measurement frequency. However, propagation 
of this point impacts the a priori mean and covariance over any interval; a longer interval 
makes the physical error’s effect more pronounced. Although this initial condition is not 
the exact cause for the filter divergence seen in Figure 2.19 s , it reveals the fundamental 
challenge of applying the UKF to physical problems subject to state constraints. The re¬ 
mainder of this chapter will investigate use of the UKF with inequality constraints to allow 
for extending the time interval between measurements (i.e., extending the maximum mea¬ 
surement interval in excess of 0.5Hz measurement frequency in the falling body problem). 
Any UKF application with implicitly constrained states may benefit from this analysis. 

Julier and Uhlmann [11], [12] noted that the UKF algorithm “naturally lends itself to being 
used in a ‘black box’ filtering library.” “Appropriately defined inputs and outputs” [11] 
divergence occurs later in the state trajectory. 
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Figure 2.20. Initial sigma point propagation, falling body problem 

are necessary, but the UKF algorithm does not enforce state constraints. The effect of 
a sigma point that contains state values outside of physical limits is not readily apparent 
in many applications. Relatively short measurement intervals limit the likelihood that a 
single sigma point violating physical constraints will overwhelm the other sigma points 
that satisfy the constraints. However, the post-transformation statistics are impacted for 
even short measurement intervals. The nonlinear function’s nature is a significant factor 
in determining the magnitude of the effect. The magnitude of the estimated velocity that 
results from integration of the process model in the falling body example is the cause of the 
UKF failure. 

Background on constrained UKF implementation is provided in the following section. Sec¬ 
tion 2.5 details a novel approach to satisfy state constraints through application of the scaled 
UT [34] to lengthen the measurement interval. Section 2.5.4 and Section 3.1 demonstrate 
the UKF-S on the falling body problem and the pendulum problem, respectively. 

2.4 Constrained UKF Background 

The UKF is known to be a suboptimal estimation technique that approximates the minimum 
mean square estimate [11]. For this reason, Simon [38] notes, “if there are additional 
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constraints beyond those explicitly stated in the system model, then the complete system 
description is different than that assumed by the standard Kalman filter equations,” that 
the KF-based nonlinear estimation techniques may benefit from incorporating constraints. 
The literature provides methods to enforce both equality and inequality constraints. The 
following sections describe different approaches to the inequality constrained UKFs in the 
literature. 

Teixeira et al. [39] note that the challenge of incorporating restraints arises in many non¬ 
linear estimation problems but is an intractable challenge for linear systems, as well. In¬ 
equality constraints are inherently violated by assuming Gaussian noise in the filter con¬ 
struct [39]. This has led to the development of approximate techniques that seek to produce 
a state estimate respecting the known constraints. Simon’s survey paper [38] presents three 
approaches; these methods involve projecting the estimate onto the constraints, truncating 
the PDF, or using a MHE approach involving formulation of a non-recursive constrained 
quadratic program. Many variations exist into the timing, either a priori or a posteriori , 
for applying the first two techniques on sigma points, as discussed in the following sec¬ 
tions [38]. One common theme among these approaches is that the estimated mean or the 
estimated mean and covariance are changed, if necessary, to satisfy the inequality con¬ 
straints. 

2.4.1 Projection 

The unconstrained estimate, xt, results from the application of the KF structure, which 
does not consider limitations on possible state values. State constraints effectively limit 
the potential region where the estimate may lie. Therefore, one approach to finding a 
constrained estimate, xt , is to find the minimum constrained estimate that satisfies the 
constraints as shown in Equation (2.12) [9]. 

X)Icon = ar § min { Kcon ~ X Y^(K,con ~ *) Dx con < d (2.12) 

xt 

k,con 

W is a positive definite weighting matrix that can be selected based on the desired con¬ 
strained estimate. Equation (2.12) projects the unconstrained estimate into the constrained 
space. Simon [9] notes that setting W to the identity matrix, /, produces the least mean 
square constrained estimate. Furthermore, setting W to the inverse of the covariance ma- 
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trix, Pf 1 , produces the minimum variance estimate [9]. The resulting quadratic program¬ 
ming problem seeks to minimize the weighted squared difference between the constrained 
and unconstrained estimates [9]. 

Teixeira et al. [39] present the sigma-point interval unscented Kalman filter (SIUKF) as a 
construct based on the interval unscented Kalman filter (IUKF). 9 This approach uses sigma 
points that do not violate the state’s inequality constraints. This condition is achieved by 
projecting any violating sigma points onto the constraints as shown geometrically in Fig¬ 
ure 2.21. Teixeira et al. [39] note that the sigma points “weighted sample mean and covari- 
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Figure 2.21. Projected sigma points for the mean and covariance shown in 
Table 2.2 


ance may not capture” the a priori mean and covariance. This effect is clearly visible in 

9 The IUKF is Teixeira et al.’s name for the sigma point selection approach used in Vachhani et al.’s 
unscented recursive nonlinear dynamic data reconciliation (URNDDR) detailed in this section [39], [40], 
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Figure 2.21. Inequality constraints are enforced on individual sigma points at both the pre¬ 
diction and update steps of the UKF algorithm. Other authors [41], [42] proposed similar 
projection approaches to constrain sigma points. 

Teixeira et al. also present the constrained unscented Kalman filter (CUKF) in [39] that 
combines a normal UT prediction step while enforcing the inequality constraints only on 
the mean during the update step. The error covariance is not constrained in this algorithm. 
The constrained interval unscented Kalman filter (CIUKF) uses the prediction step of the 
SIUKF coupled with the update step of the CUKF [39]. The IUKF uses the prediction step 
of the SIUKF and CIUKF coupled with the UKF update step [39]. 

Vachhani et al. [40] present the URNDDR to incorporate constraints on sigma points prior 
to propagation. This method generates sigma point vectors that are asymmetric and alters 
the associated weights to account for the asymmetry. Vachhani et al. [40] note that the 
sigma point vectors of Julier and Uhlmann’s extended symmetric set have components 
that lie a distance of + k in the direction + JPk\k- The URNDDR instead employs 
sigma point vectors that have components in the same direction, ± yj Pk\k, but at a different 
distance, as calculated in Equation (2.13) [40]. 


0 ,k = min( + k,O ik, 02 k) 

Oik = min (°°, Xu,j ~ *k\kj) 

j-Sij> o 

Oik = min (oo,xlj - x k \k,j) 

J<0 

The associated weights are calculated, as shown in Equation (2.14) [40]. 


(2.13) 


Wj = adj + b, where 


2(n+K)(Sg-(2n+l)( V^+/c)) 

b= _L_ 2*=1 _- 

2 (n+K) 2's/n+K(Sff-(2n+l)( ^n+K.)) 

2 n 

Se= Z0i 

i=i 


(2.14) 


These constrained sigma points are propagated forward in time, and a constrained opti¬ 
mization problem involving Equations (2.13) and (2.14) is solved for each sigma point. 
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Each resulting sigma point vector, Xk+\\k+u contain components that satisfy all bounds. 
This ensures that the estimate is in the allowable region as required to generate the next 
time step’s sigma points using the method shown in Equation (2.13) [40]. 

2.4.2 Truncated Probability Density Function 

Details of implementing a constrained UKF using PDF truncation are found in [9]. This 
approach centers on the assumption that the PDF is known Gaussian. The estimated co- 
variance matrix is transformed into a diagonal matrix so that constraints can be applied 
separately to each state. The unconstrained PDF is truncated at each states’ constraint 
boundaries. The state estimate is the centroid of the truncated PDF. The inverse of the 
transformation is applied to the normalized covariance of the truncated PDF to produce the 
constrained estimated covariance [9]. This approach assumes that the PDF remains Gaus¬ 
sian despite nonlinear transformation and, therefore, may be subject to significant error. 

Teixeira et al. [39] apply this approach to the UKF and IUKF as the truncated unscented 
Kalman filter (TUKF) and truncated interval unscented Kalman filter (TIUKF), respec¬ 
tively. These approaches are three-step estimators in that the prediction and update step are 
performed in the usual fashion. A third step, the truncation step, is included to determine 
the estimated state and covariance. The a posteriori mean and covariance are subjected to 
the inequality constraints such that a new constrained mean and covariance are determined 
if any constraint is violated [39]. The truncated mean and covariance are then used as the 
next time step a priori mean and covariance [39]. 

2.4.3 Moving Horizon Estimation 

As noted in Chapter 1, the MHE was specifically developed to incorporate state constraints 
into nonlinear estimators [23]. This approach reformulates the estimator as an optimal 
control problem with constraints. The key insight was to limit the horizon over which 
optimization is required by accurately reflecting knowledge prior to the horizon as an arrival 
cost [23]. This approach cannot be employed with the KF structure. 
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2.5 Scaled UKF 

This section investigates the new idea of using the Julier [34] sigma point scaling technique 
to achieve filter convergence over longer measurement intervals. The key insight leading to 
development of the UKF-S is that parameters are often constrained in model formulation. 
As a result, the improved model accuracy achieved through parameter estimation can be 
lost if sigma point vectors violate parameter constraints. Scaling is used to ensure that the 
a posteriori estimate and associated sigma points respect inequality constraints. 


2.5.1 Scaled Unscented Transform Background 

As the dimension of the state space increases, the radius of the hyper-sphere bounding the 
sigma points also increases. As a result of non local effects, transforms performed using 
the sigma point set will suffer in accuracy [34]. Julier et al. [43] proposed an approach to 
reduce the non local effects in the symmetric sigma point set presented in [13]. Julier [34] 
presents a more general approach, the scaled UT, to alter any sigma point set including the 
mean vector, xo = Ax-. 10 . The scaled UT was proposed to minimize sigma point non local 
effects by reducing the sigma point vector bounding hyper-sphere radius while preserving 
the first two moments of the set. 


Scaling is accomplished using a scale factor, a e (0,1], with an affine transformation of 
the sigma points shown in Equation (2.15). 


Xo = Xo 

X'i = Xo + a(xi ~ Xo), i = 1,. • • ,2/7 


K = ^t + o- 

a z 


a- 


VY I O ’ *■ 


1 , 


,2/7 


or 


(2.15) 


When a = 1 the scaled sigma point vectors and weights are equivalent to the unsealed 
sigma points. Julier [34] noted that, in addition to reducing non local effects, this technique 
allows for the incorporation of higher order information that can improve the accuracy of 
the unscented transformation. 

10 Any sigma point set can be extended by including the vector, xo = Hx with Wo = 0. 
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Figure 2.22 shows the effect of scaling the extended symmetric sigma point set represent¬ 
ing, x, a representative two-dimensional Gaussian joint PDF with fi x and P x , as shown in 
Equation (2.16). 

[5 


1 2 
2 8 


(2.16) 


A common scale factor, a e {1,0.5,0.1}, is applied to demonstrate the effect on sigma 
point vector location. The mean sigma point vector, xo, is not impacted by the scaling. 
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Figure 2.22. Comparison of scaling sigma points representing, x, a two- 
dimensional Gaussian joint PDF. 


Careful inspection of Figure 2.22 reveals that the scaled sigma points are not equidistant 
about the mean, xq, but rather are located xq± a * \Jn + k * yfP, where n = 2 and k = 1 
in this example. The \[J\ provides the direction along which the bounding hyper-sphere 
radius, R = a * yfn + k, extends. 
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Figure 2.23, when viewed in conjunction with Figure 2.21, shows a geometric represen¬ 
tation of the difference between scaling and projecting sigma point vectors to satisfy con¬ 
straints. 



Figure 2.23. Scaled sigma points for the mean and covariance shown in 
Table 2.2 


The projected sigma point vectors are moved over the shortest distance to the constraint 
boundaries, unlike the scaled sigma point vectors. Although this projection approach en¬ 
sures that the sigma point vectors respect constraints, the represented first and second mo¬ 
ments are altered, as shown in Table 2.2. 

The scaled UT preserves the first and second moments of a sigma point set while reduc¬ 
ing the radius of the hyper-sphere encompassing the sigma point vectors [34]. The scaled 
UT also preserves the third moment in this case and scales the fourth moment. Higher- 
order moments of the sigma point set are altered in exchange for reducing the hyper-sphere 
radius, as shown in Table 2.2. Since systems with nonlinear dynamic models do not neces¬ 
sarily maintain a Gaussian PDF throughout the entire state trajectory, the potential benefit 
of matching Gaussian third and higher order moments may be limited in comparison to the 
benefit of respecting constraints. The following section details an UKF that employs scaled 
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Table 2.2. Comparison of 2-dimensional example of projected (Figure 2.21) 
and scaled constrained (Figure 2.23) sigma points. 



1 st Moment 

2 nd Moment 

3 rd Moment 

A th Moment 

Original 

5 

2 

1 

2 

2' 

8 

i i 

o o 

L J 

'15 

30 

30' 

108 

Constrained 

'5.1' 

0.65 

0.91' 

0.62 

'3.97 

5.50' 

(Projected) 

2.0 

0.91 

2.67 

0.44 

5.50 

13.25 

Constrained 

5 

1 

2' 

O' 

'5 

10' 

(Scaled) 

2 

2 

8 

0 

10 

36 


sigma points to achieve convergence despite long measurement intervals. 

2.5.2 Scaled UKF Concept 

The UKF-S employs two scale factors to respect parameter constraints. The first scale 
factor, a, is the common scale factor used in the scaled UT, selected to ensure that all 
sigma point vectors are located in the allowable region. The allowable region is a space 
in which all points satisfy the parameter constraints along with an offset to account for a 
minimum sigma point bounding hyper-sphere radius, R m i n . The minimum distance ensures 
that the sigma points will adequately represent the PDF through the transformation process. 

Propagation of a Gaussian state through a nonlinear process provides some insight as to 
the necessity to specify a minimum hyper-sphere radius. The covariance matrix shown in 
Equation (2.17) is propagated forward for 2 seconds using scaled sigma points. 

1 x 10 6 0 0 

0 1x 10 6 0 

0 0 2.5 x 1(T 9 

0 0 0 

Propagation commences from state values at the specified to along the falling body state 
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trajectory to demonstrate the effect sigma point scaling has on covariance transformation. 
These sigma points do not require scaling to satisfy state constraints. However, transfor¬ 
mation of the scaled sigma points demonstrates the impact of reducing the hyper-sphere 
radius. The scaled UT covariance matrix is compared with a covariance matrix propagated 
forward 2 sec using a 10,000 trial Monte Carlo. Many approaches to determine matrix 
similarity exist as summarized by Vemulapalli and Jacobs [44]. The Jensen-Bregman log- 
det (JBLD) divergence value, shown in Equation (2.18), is particularly useful to compare 
the covariance matrix similarity [44], [45]. 


Jld = -ylog |det j j - ^ log (det (XT)) (2.18) 

The JBLD technique is a computationally efficient technique applicable to positive definite 
square matrices [45]. Identical matrices will produce a JBLD divergence value of 0. The 
JBLD divergence values are used to determine the quality of the different scaled UT prop¬ 
agation. JBLD divergence values presented in Table 2.3 reveal that in regions where the 
process model is effectively linear, such as at to = 0, the scale factor has no impact upon the 
propagated covariance matrix. However, in regions of the state trajectory that are subject 


Table 2.3. Comparison of scaled UT propagated covariance matrices with 
Monte Carlo propagated covariance. 


a 


1 

0.8 

0.6 

0.4 

0.2 

to 

= 0 s 

1.2998 x HU 2 

1.2998 x Hr 2 

1.2998 x lCU 2 

1.2998 x HU 2 

1.2998 x 10“ 2 

to 

= 8 s 

1.5271 x 10“ 2 

1.5102 x 10“ 2 

1.492 91 x 10“ 2 

1.4855 x 10“ 2 

1.4841 x 10“ 2 

to 

= 10 s 

1.1009 x 10“ 2 

1.2348 x 10“ 2 

1.5065 x 10~ 2 

1.6645 x 10“ 2 

1.6949 x 10“ 2 

to 

= 12 s 

2.0880 x 10“ 2 

2.3343 x 10“ 2 

2.8994 x 10~ 2 

3.2350 x 10“ 2 

3.2999 x 10“ 2 


to significant nonlinearity, such as at to - 12, the similarity of the propagated covariance 
matrix is less for smaller scale factors. Equivalently, smaller bounding hyper-sphere radii 
have greater impact on the quality of the propagated covariance. 

The effect of the bounding hyper-sphere radius is also visible through comparing the error 
along the covariance matrix main diagonal. The percent of covariance propagation error 
between the different scaled UT propagation and the Monte Carlo propagation is shown 
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in Figure 2.24. The covariance of each state is represented by the respective covariance 
matrix main diagonal term. 
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Figure 2.24. Arbitrary covariance (Equation (2.17)) 2 sec propagation from 
the specified to falling body truth trajectory starting point: (a) depicts the 
% error comparison, to = 0 s, (b) the % error comparison, to = 8 s, (c) the 
% error comparison, to = 10 s, and (d) the% error comparison, to = 12 s. 


The scale factor, a, is conceptually determined by finding the maximum radius, R max , of a 
hyper-sphere centered on the estimate that will satisfy all constraints. 11 The state estimate 
must be relocated into the allowable region if the maximum hyper-sphere radius satisfying 
all constraints is shorter than the minimum allowable, R m i n . The minimum bounding hyper¬ 
sphere radius, R m i n , can be a constant, as demonstrated later in Section 2.5.4, or vary along 
the state trajectory. 

"State estimates located outside of the allowable region have R max — 0. 
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The required sigma point scale factor is known if R max ^ Rmin (i-C., state estimate re¬ 
location is required), a = a m! -„. Additionally, the scale factor, a = 1, is known if no 
scaling is required as occurs if R max > x/n + k . 12 In these instances, no sigma point scal¬ 
ing is required to satisfy constraints. The scale factor must be calculated in instances when 
Rmin < Rmax < V« + « using Equation (2.19). 


a = 


R 


max 


V « + K 


(2.19) 


The URNDDR sigma point selection method, detailed in Equations (2.13) and (2.14), could 
conceptually be employed in the UKF-S instead of the scaled UT. However, it is not 
used due to the greater complexity required to select the associated weights, as shown in 
Equation (2.14). Additionally, the weights generated for a given common scale factor are 
not consistent with those of the scaled UT sigma point approach. Further review reveals 
that the URNDDR approach does not produce a sigma point set that exactly matches the 
covariance if a symmetric bounding hyper-sphere is used. Rather, the URNDDR sigma 
point set for a symmetric bounding hyper-sphere matches a scaled covariance, as shown in 
Equation (2.20), which has negative implications for the quality of the post-transformation 
covariance. 


Purnddr - 


a 2 (a + 2k + 2/7 - 2a k - 2 an) 
2/7 - 2 an + 1 


( 2 . 20 ) 


A second common scale factor, K s , is used to relocate the state estimate into the allowable 
region when performing the UKF update, if necessary. The a posteriori state estimate, xt, 
of the UKF-S is formed, as shown in Equation (2.21). 


x + k = x k + K sJ( K k (y k - y k ) 


( 2 . 21 ) 


The Kalman gain, K k , is scaled by K s k , the minimum magnitude deviation from K s k = 1 
necessary, to locate the state estimate, x k , in the allowable region. K sk = 1 if scaling is not 
required (i.e., the a posteriori state estimate, x + k , is found in the same manner as the UKF 
shown in Equation (1.45)). The variance of the state estimate is determined, as shown in 

12 The threshold, \jn + k, applies for the extended symmetric sigma point set. The threshold is the radius 
of a bounding hyper-sphere encompassing all sigma points in a set. 
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Equation (2.22), using the same common scale factor, K s ^, for the mean and covariance. 


P + k = P~ k - (K sk K k )P y (K s , k K k ) r (2.22) 

Kalman gain scaling does not necessarily reduce the optimality of the estimate since the 
UKF is known to be a suboptimal estimator for nonlinear estimation. 

2.5.3 Scaled UKF Implementation 

The scaled UT demonstrated in Section 2.5.4 is implemented without determination of 
the maximum allowable bounding hyper-sphere, R max ■ This approach avoids the need to 
perform the computationally intensive nonlinear estimation problem to calculate R max each 
update step. The allowable region is instead defined as the space bounded by the parameter 
constraints and an additional specified offset from the constraints referred to as a “guard 
band”. The guard band is established to ensure that the sigma point scaling will not result 
in a scale factor, a < a m j n . This approach can only be applied if the mean itself is in 
the allowable region as discussed in Section 2.5.2. The a posteriori mean is positioned, if 
necessary, into the allowable region through use of the Kalman gain scaling discussed in 
Section 2.5.2. 

The scaling factor, an, is selected, as shown in Equation (2.23), V i = l,... ,N where N is 
the number of sigma point vectors, and j is the constrained state. 

Constraint - X oj „ 

OH - - (2.23) 

Xij - xoj 

Scaling occurs if any a ; 6 (0,1]. The smallest a,-, is used for the scaling of Equation (2.15) 
as a common scale factor to ensure all sigma points remain within the constraints. If all 
a, > 1, scaling is not necessary to satisfy the state constraints. This approach maintains the 
symmetry of sigma points about the mean. The affine transformation of the original sigma 
points and weights, shown in Equation (2.15), ensures that the mean and covariance of the 
scaled sigma point set is not altered, as discussed in Section 2.5.2. Figure 2.25 graphically 
shows the UKF-S scale factor selection process. 

The scaled UT can be applied to calculate the UKF a priori estimate using Equation (1.41). 
Parameters will remain within constraints throughout the prediction step since they do not 
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Figure 2.25. Two-dimensional example of UKF-S scaling, a = 0.289 required 
for all sigma points to satisfy the constraints. 

have process dynamics. Integration of state process dynamics can still lead to state con¬ 
straint violation during propagation to the measurement time. The scaled UT can also be 
applied to calculate the predicted measurement using Equation (1.42), if necessary. 

2.5.4 Simulation Results 

The results presented in this section include adaptive scaling that maintains all sigma points 
X 3 > 0 but does not enforce the physical limits of the other states. A X 3 £ on constraint, 
X 3 > 1 x 10 -5 , is employed to enforce the physical limit. Additionally, a X 3 guard band, 
x 3 ,Guard = 1 x 10 -5 , to allow for the propagation of the sigma points as described in the 
previous section. Table 2.4 presents the sigma points of Table 2.1 following scaling to 
respect the ballistic coefficient, *3, physical constraint. Table 2.4 is generated assuming 
filter initialization parameters of Section 2.2. The weighted mean and covariance of both 
sets of initial sigma points are equivalent. The higher order moments associated with each 
set differ. 

Figure 2.26 presents the results of a 100-trial Monte Carlo study, comparing the EKF2, 
UKF, and UKF-S using the same measurements as Section 2.3 sampled at the specified 
frequencies. The UKF-S adaptive scaling technique improves UKF robustness while ap- 
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Table 2.4. Falling body example of Section 2.2 In + 1 scaled (a = 0.4995) 
sigma point set at t = 0. 


Sigma Points (2n+l) 

1 

2 

3 

4 

5 

6 

7 

8 

9 

Weight 

0 

0.125 

0.125 

0.125 

0.125 

0.125 

0.125 

0.125 

0.125 

Altitude, X{ (ft) 

300000 

300999 

300000 

300000 

300000 

299001 

300000 

300000 

300000 

Velocity, X 2 (ft/s) 

20000 

20000 

21998 

20000 

20000 

20000 

18002 

20000 

20000 

P. ^3 (1/ft) 

0.01 

0.01 

0.01 

0.01999 

0.01 

0.01 

0.01 

1.00 X 10" 5 

0.01 

g, x 4 (ft/s 2 ) 

32.17405 

32.17405 

32.17405 

32.17405 

32.18404 

32.17405 

32.17405 

32.17405 

32.16406 



Time(s) 



Time(s) 
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Figure 2.26. Comparison of EKF2, UKF, and UKF-S estimation perfor¬ 
mance, 100 trial average: (a) depicts average absolute altitude (;ci) error, 2 
Hz measurements, (b), average absolute altitude (xi) error, 1 Hz measure¬ 
ments, (c), average absolute altitude (jci) error, 0.5 Hz measurements, and 
(d) average absolute altitude (.xi) error, 0.2 Hz measurements. 
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parently slowing the convergence rate at higher measurement frequencies. The UKF-S 
performs nearly identical to the EKF, EKF2, and UKF at the sufficiently fast measurement 
rate of 2 Hz. Sigma point scaling is required at the first 8 propagation intervals for 1 Hz 
measurements, the first 5 propagation intervals for 0.5 Hz measurements, and the first 3 
propagation intervals for 0.2 Hz measurements. Kalman gain scaling is not necessary for 
this example since the state estimate for the ballistic coefficient, * 3 , remains in the allow¬ 
able region. 

Tuning may be required as selection of different lower bound and guard bands may improve 
performance. The UKF-S converges at the 0.2 Hz measurement rate, Figure 2.26d, but 
fails to converge at longer intervals. Inspection of the sigma points at longer measurement 
intervals reveals that while X 3 remains within bounds, other states violate their respective 
physical limits. Additional state constraints can be incorporated into selecting the common 
scaling factor to ensure that sigma points respect physical bounds prior to propagation. 

2.6 Conclusions 

The literature contains numerous comparisons between EKF and UKF performance in ap¬ 
plication. Some authors have noted that the EKF and UKF often produce similar results and 
recommend use of the EKF based on reduced computation requirements. Their analyses do 
not account for the potential effect of measurement interval on estimator performance. As 
demonstrated in this chapter, the length of the measurement interval can impact estimator 
performance. While performance may be similar between the EKF and UKF at sufficiently 
high measurement frequencies, the UKF demonstrates greater tolerance to lengthened mea¬ 
surement intervals in the single-dimension falling body problem considered here. 

Differences in the KF approximation techniques employed by the EKF and UKF is masked 
at relatively fast measurement rates. The impact of the approximation method is most obvi¬ 
ous in instances when the covariance is significantly underestimated and the measurement 
interval is lengthened. This occurrence leads to the filter rejecting information from succes¬ 
sive measurements. Once this occurs, the estimate tracks the predicted estimate, rejecting 
information from the measurements, and resulting in filter divergence. Process noise can 
reduce this effect by offsetting the covariance underestimation at the expense of reducing 
filter effectiveness. In instances when the covariance is overestimated, the filters do not 
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diverge, but will track the measurement, reducing filter effectiveness. 

Sufficiently fast measurements lead to nearly identical performance between EKF, EKF2, 
and UKF implementations in the single dimension falling body problem considered in this 
chapter. However, the UKF is shown to outperform the EKF with sparse temporal mea¬ 
surements in this example. The difference in performance results from the propagation of 
the estimated covariance. 

The UKF’s failure when subjected to measurement frequencies less than 0.5 Hz, as detailed 
in Section 2.3.5, required consideration of a constrained UKF. A novel constrained UKF, 
the UKF-S, was presented in Section 2.5 to employ the scaled UT to increase the inter¬ 
val between measurements while using the UKF. This approach demonstrated the ability 
to lengthen the measurement interval in the example considered by using a constrained 
nonlinear estimator. 

Chapter 3 will seek to confirm the results of this chapter through experimentation by con¬ 
sidering another nonlinear estimation problem, a simple pendulum. 
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CHAPTER 3: 
Experimental Investigation 


A simple pendulum problem is used to explore the effect of measurement frequency on 
KF-based nonlinear estimators through concurrent experiment and simulation. Section 3.1 
provides a detailed description of the simple pendulum problem that is studied through sim¬ 
ulation and experimentation. The experimental setup is described in detail in Section 3.2. 
The simulation is described in detail in Section 3.3. Results from simulation, Section 3.4, 
and experiment, Section 3.5, are presented for comparison to determine if measurement fre¬ 
quency impacts estimator performance in this problem and provide experimental validation 
of the simulation. Conclusions are presented in Section 3.6. 

3.1 Pendulum Example 

The pendulum system shown in Figure 3.1 is investigated to determine the impact of mea¬ 
surement frequency on the performance of KF-based nonlinear estimation algorithms. This 



example was selected for two reasons. First, it is a periodic nonlinear system that has a 
well-understood model. Second, it allows for the setup of a simple laboratory experiment 
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for validation of the effect of measurement frequency on filter performance. The system is 
described by first-order ordinary differential equations with a two-dimensional state space, 
as shown in Equations (3.1) and (3.2) [46]. The pendulum arm angle, 6 (rad), and the 
pendulum arm angular velocity, co (rad/s), form the state space used to describe motion of 
the pendulum bob. An ideal, or mass-less arm, pendulum model is initially considered for 
simulation. 


e 

CO 


(3.1) 



*2 


0 

X = 

- ( lm ) g sin(.vi) - f3x 2 

+ 

W2 ~ N (0 ,Q 2 ) 


hob + ml 2 



The pendulum arm length, / (m), the pendulum bob mass, m (kg), and gravitational ac¬ 
celeration, g (m/s 2 ), can be measured rather accurately while the system is stationary. The 
bob’s moment of inertia, hob (kg m 2 ), and the system damping ratio, (3 (kg m 2 /s), may be 
challenging to directly measure. These terms can introduce significant model error result¬ 
ing in poor estimation of the system state. All damping forces to include sliding friction at 
the pendulum pivot, air resistance, etc. are represented by f3. The moment of inertia and 
damping coefficient are expected to remain constant during the interval between measure¬ 
ments and are treated as parameters. The state space can be augmented with these estimated 
parameters, as shown in Equation (3.3), to enhance the model fidelity. 


e 

CO 

IBob 

P 


(3.3) 


The resulting system dynamics are represented by Equation (3.4). The additive process 
noise included in this equation is used to account for any anticipated model inaccuracy. 
The noise associated with X 2 and the estimated parameter, X 4 , are uncorrelated since u >2 
only accounts for the inaccuracy resulting from the parameters (/, m, and g ) that are not 
estimated along with additional modeling inaccuracies not accounted for by the estimated 
parameters. The X 3 parameter, I Bob, does not have any noise associated with it since it 
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is not expected to change during the interval of interest. The X 4 parameter, the damping 
coefficient, may change during the interval of interest, hence the addition of the process 
noise term, W4. 


it 


X2 



0 

X2 


- ( lm ) g sin(.vi) - X 4 X 2 


W 2 ' 

“JV(0,02) 

= f(x(t)) = 

a '3 + ml 2 

+ 

X3 


0 



0 

X 4 


0 


W 4 - 

“Af(0 ,Q 4 ) 


The measurement model is linear and discrete in this example, as shown in Equation (3.5). 


y(t k ) = [ 1 0 0 0 ] 


Xl (tk) 


Vl(t k ) 

-N(0M ' 

X2 (t k ) 

+ 


0 

X3 (tk) 


0 

X4(tk) . 



0 


(3.5) 


Measurements are assumed to include an additive noise, v\ (t k ), that is Gaussian and uncor¬ 
related with any process noise. The measurement model satisfies the linearity requirements 
of the KF. The process model, however, is nonlinear; therefore, the estimate is not optimal 
when using the KF structure. 


A second model, the physical pendulum, shown in Equation (3.6), is also considered to 
better reflect the experimental setup where the pendulum arm has mass and therefore affects 
the system dynamics [46]. The pendulum arm is considered a cylindrical tube allowing 
application of the standard equation, I Arm = h Cylinder = (3r 2 + /? 2 j where m is the mass 

of the tube, r = r Arm is the pendulum arm radius, and h = ItArm is the length of the 
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pendulum arm from the top of the bob to the pendulum pivot [47]. 


*2 


0 

/ / h j\yyh \ \ 


lm Bob + 0 m Ann 9 sin(.vi) x 4 x 2 



\ \ 2 I / 


w 2 ~ N(0,Q 2 ) 

2 


X3 + tJlBobl~ "t" 1 Ann + Ann ( 2 ) 

+ 

0 

0 


w 4 ~ N(0,Q 4 ) 

0 




The following simulations assume that the filter is initialized to an estimate that reflects 
the actual state, as shown in Equation (3.7). xi is the arm angle at t = 0 s. An initial 
deflection of approximately 0.698 rad (40 deg) is selected so that the pendulum is not in an 
equilibrium position and that a small angle approximation for sin{x\) is a poor assumption. 
The pendulum is assumed stationary at t = 0, co/to) = 0 rad/s. *3 is the calculated bob 
moment of inertia, 2.758 x 10 -3 kgm 2 , assuming the parameters from Table 3.1. The bob 
is assumed to be a cylinder of uniform density allowing the application of the standard 
equation, l Bo b = Icylinder = jk (3r 2 + h 2 j, where m is the mass of the bob, r is the bob’s 
radius, and h is the bob’s height [47]. X 4 is estimated as j3o = 0.015 kgm 2 /s, 150% of the 
value obtained by experimentation, as discussed in Section 3.2. 


x(t 0 ) = 


6 (to) 

0 

IBob 

A> 


(3.7) 


The initial covariance matrix, P(to), shown in Equation (3.8) is selected to reflect uncer¬ 
tainty in the initial state estimate. Despite appearing identical, the variance of the param¬ 
eters, *3 and a' 4 , is relatively larger than that of the angle and angular velocity due to the 
respective units and reflects the inherent uncertainty in the calculated initial estimate. The 
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same variance is used for all filters in simulation and experimental comparisons. 


P(to) = 


1 x 10“ 4 
0 
0 
0 


0 

1 x 10“ 4 
0 
0 


0 0 

0 0 

1 x 10“ 4 0 

0 1 x 10“ 4 


(3.8) 


3.2 Experimental Setup Details 

The pendulum experimental setup is shown in Figure 3.2. Experiment parameters are pre¬ 
sented in Table 3.1. This data was obtained by measuring the physical parameters associ- 

Table 3.1. Measured pendulum experiment physical parameters. 


Parameter 

Value 

/ 

405.0 mm 

f Bob 

46.0 mm 

hBob 

65.0 mm 

b Arm 

6.3 mm 

mBob 

3.13 kg 

HI Ann 

322.6 g 


ated with the pendulum experiment. Length measurements less than 200 mm was obtained 
using calipers with accuracy of ±0.03 mm. Length measurements in excess of 200 mm was 
obtained using a tape measure with accuracy of +0.5 mm. Mass was obtained using a scale 
with accuracy of ±1 g. 

An optical encoder and a potentiometer sense pendulum arm angle simultaneously. The 
encoder data is an accurate, essentially noiseless measurement signal. The potentiometer 
data provides a noisier measurement for comparison. 

Data is acquired using an Arduino Uno board and Mayhew Engineering 16-Bit Extended 
Analog-to-Digital Shield, shown in Figure 3.3, to perform the potentiometer analog to 
digital conversion. It also transmits the digital encoder and potentiometer output via a 
serial connection to a computer for storage and offline estimation. As noted in Section 3.3, 
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(a) (b) 


Figure 3.2. Pendulum experimental set-up: (a) depicts the pendulum exper¬ 
iment, front view, and (b) the pendulum experiment, top view. 

these two sensors are selected to provide an accurate measurement of the pendulum arm 
angle along with a less accurate measure. Additionally, these sensors provide representative 
angle measurements that are commonly available for control systems. 

The optical encoder is a Computer Optical Products, Inc. CP-350-4096 digital incre¬ 
mental optical encoder. It has 16,384 counts per revolution producing a resolution of 
3.835 x 10 -4 rad/count (2.197 x 10 -2 deg/count). 
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Figure 3.3. Arduino Uno with Mayhew Engineering 16-Bit Extended Analog- 
to-Digital Shield 

The potentiometer is a RadioShack 5 kQ ± 20% linear-taper potentiometer with a nominal 
total rotation of 300 ± 5 deg. This potentiometer has an actual resistance of 4.54 k£2. 
The potentiometer is connected to the Arduino Uno’s 5 V power supply. The 16-bit 
analog to digital converter has 65,356 counts. This results in a theoretical resolution of 
7.99 x 10 -5 rad/count (4.58 x 10“ 3 deg/count) when used with a 5 V power supply, nearly 
an order of magnitude better resolution than the encoder. The potentiometer was rotated 
50 times from hanging stationary through 90 deg of travel as measured in encoder counts 
(4096 counts). This data is used to determine the radian per potentiometer count con¬ 
version to allow for comparison between the potentiometer and encoder. A resolution of 
6.95 x 10 -5 rad/count (3.98 x 10 -3 deg/count) is obtained. 

The potentiometer measurement is corrupted by both electrical and mechanical noise. Elec¬ 
trical noise associated with the potentiometer signal was determined using an oscilloscope, 
as shown in Figure 3.4. The signal displayed random noise with a standard deviation of 
approximately 1 mV while the potentiometer is stationary. This equates to a standard de¬ 
viation of 9.10 x 10 -4 rad (5.22 x 10 -2 deg). This noise level requires a 16-bit analog to 
digital converter to provide resolution necessary for sensing the Gaussian noise signal. 

The potentiometer is subject to hysteresis from the slotted coupling connecting the poten- 
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Figure 3.4. Electrical noise in potentiometer voltage output detected using 
an oscilloscope. 

tiometer wiper to the pendulum pivot. Additionally, the wiper is subject to static friction 
that must be overcome before angular change is detected. Figure 3.5 shows an example of 
the data collected using this experimental set-up. A standard deviation of 159.05 counts 
over the 50 trial runs, approximately 1.11 x 10 -2 rad (6.33 x 10 -1 deg), was observed 
when the potentiometer is connected to the 5 V power supply. This value includes mechan¬ 
ical noise in addition to the electrical noise. 

The angle measurement is calibrated by zeroing the angle measurement with the pendulum 
motionless, hanging down in the lower equilibrium position. This calibration method en¬ 
sures gravitational acceleration is acting in the direction of the pendulum arm with a 0 rad 
arm angle as assumed in the modeling process. 

Angle data is recorded off of the serial bus in a continuous stream at 250000 baud with 
the message containing a time stamp, encoder counts, and potentiometer counts. This data 
collection system produces an average 947 Hz measurement signal. The measurement 
frequency is irregular and varies from 880 Hz to 1147 Hz. The stored measurement stream 
is sampled offline at the average desired frequency to investigate filter performance at lower 
measurement frequencies. Sampling is performed by using the measurement nearest to the 
desired time vice using interpolation. This approach allows studying the impact of sparse 
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temporal measurements while maintaining measurement independence. 

The gravitational acceleration constant is assumed to be equal 9.81 m/s 2 . The data from 
Figure 3.5 is used to determine an approximate damping coefficient for initialization pur¬ 
poses. The damping coefficient, * 3 , is approximated as 0.01 kgm 2 /s by using Figure 3.5a 
in conjunction with the physical pendulum model to match the angle amplitude lost during 
each period. 

Figure 3.5b reveals substantial hysteresis in the potentiometer signal. This effect is not 
accounted for in the linear measurement model specified in Section 3.1. The potentiometer 
hysteresis must be accounted for by either altering the model or increasing the measurement 
noise in order to use the potentiometer information in the EKF or UKF. 

A single trial of the experiment is conducted to collect the sensor data. The encoder angle 
information is used as accurate measurement information since it is subject to significantly 
less noise than the potentiometer signal. The desired measurement frequency is obtained by 
sampling the sensor data at intervals that average to the specified frequency while running 
the filters offline. This approach results in irregular measurement intervals. All filters are 
initialized using the initial encoder angle from the experimental data and an initial angular 
velocity, X 2 (/o) = 0 rad/sec. An initial angle of approximately 0.6 rad (35 deg) is used to 
ensure a nonlinear model is required to model the system state. 
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(b) Pendulum angle measurements, expanded view 



Figure 3.5. Representative trial comparing simultaneous encoder and poten¬ 
tiometer pendulum angle measurements: (a) depicts pendulum angle mea¬ 
surements (rad), (b) an expanded view of pendulum angle measurements, 
(rad), and (c) encoder versus potentiometer error pendulum angle measure¬ 
ments. 
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3.3 Simulation Details 


A simulation of the simple pendulum is performed to determine the measurement fre¬ 
quency at which estimator performance is affected. The system of equations shown in 
Equation (3.9) is propagated in time to generate a truth model using MATLAB’s ode 45 
function. 


x = 


X 2 


l in Bob + l -— ~z—— in Arm I <3 sin(xi) - (3X2 


3Total 


(3.9) 


The total system moment of inertia, hotal, is calculated using Equation (3.10) with the data 
from Table 3.1, 5.31 x 10 _1 kgm 2 ; (3 is setatO.Ol kgm 2 /s as experimentally approximated 
in Section 3.2. Assumed initial conditions are x\ - 0.873 rad; X 2 = 0 rad/s. Figure 3.6 
shows the truth model for all simulations of this example. 


1Total ~ lBob ml + I Ann + m 


l ~ llBob \ 
2 




(3.10) 


The simulation is based on the physical pendulum model to more closely approximate the 
experimental setup, facilitating validation of the simulation results. Although the ideal 
pendulum model produces a very similar path, as seen in Figure 3.7a, the two models are 
actually out of phase as a result of the physical pendulum model’s larger total moment of 
inertia altering the period of oscillation. The model difference results in significant angu¬ 
lar position differences that are shown in Figure 3.7b. This illustration should be consid¬ 
ered when analyzing the performance of the filtering algorithms in practical applications as 
demonstrated in Section 3.5. 

The generated true angle data, shown in Figure 3.6a, is corrupted with an additive ran¬ 
dom noise to generate noisy measurements. Noisy measurements for the accurate po¬ 
sition information used in Section 3.4 are generated using an additive Gaussian noise, 
~ N (0 rad, 1.324 x 10 -6 rad 2 ). The noise strength is selected to account for noise 3 
times the resolution of the encoder used in the experimental setup (3.835 x 10 -4 rad/count). 
Noisy measurements for the poor position information shown in Section 3.4 are generated 
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Figure 3.6. Pendulum problem, truth generated by propagating initial con¬ 
ditions: (a) depicts angle (xi) and (b) angular velocity ( X 2 ). 


using an additive Gaussian noise, ~ N(0 rad, 1.000 x 10 -4 rad 2 ). The variance informa¬ 
tion calculated in Section 3.2 for the potentiometer, using 5 V input and applied as noise 
in simulation, does not replicate the hysteresis and lag visible in Figure 3.5b. It does, how- 
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Figure 3.7. Comparison of truth generated by propagating initial conditions 
using the ideal versus physical pendulum models: (a) depicts the ideal model 
angle, x\ (rad) and (b) physical and ideal pendulum models, angular differ¬ 
ence (rad). 


ever, provide some sense of filter convergence with noisy measurement information. Two 
levels of measurement noise are considered since noisier measurements place a greater bur¬ 
den on the model to more accurately predict covariance in order to properly weight poor 
measurements. 

A 100 trial Monte Carlo study is conducted using a set of 100 independent trials of both 
accurate and poor measurement information. Each trial has a unique measurement signal 
corrupted by uncorrelated noise. Each measurement signal is generated at 1000Hz to allow 
sampling at varied measurement frequencies. Figures 3.8a and 3.8b noisy measurements 
sampled at 100 Hz are a representative example of the measurement error used within each 
individual trial. Each filter is tested using the same set of 100 trials to allow for meaningful 
performance comparison. 

The different measurement frequencies investigated are produced through sampling the 
1000Hz noisy measurement data at the desired rate. Three filters are compared; the EKF, 
EKF2, and UKF are each initialized using the same values as in Section 3.2. Process noise 
is not included in Section 3.4 since the physical process model was used to generate the 
truth plot for these simulations. Average absolute error in the state estimate is used to 
compare filter performance. 
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(a) (b) 


Figure 3.8. Representative noisy measurement error: (a) depicts accurate 
noisy measurement error, 100 Hz and (b) poor noisy measurement error, 
100 Hz. 


3.4 Simulation Results 

Simulation results presented in this section are generated with the EKF, EKF2, and UKF 
assuming the physical pendulum truth model shown in Equation (3.6). Process noise, W 2 
and W 4 , is not added since the truth model and the assumed filter model are identical. 
Results are presented at two frequencies, 100 Hz and 1 Hz, to determine if the measurement 
frequency effect seen in the falling body problem is present in the pendulum system. The 
1 Hz frequency is presented to ensure that the pendulum motion is sampled at least once 
each period. 


3.4.1 Accurate Measurements 

These results are produced using measurements with Gaussian noise, ~ N{0 rad, 
1.324 x 10~ 6 rad 2 ), representing optical encoder measurement noise. The average abso¬ 
lute error plots shown in Figures 3.9 and 3.10 reveal that in contrast to the falling body 
problem in Section 2.3, the three filtering techniques generated nearly identical results at 
both frequencies tested. 

Additionally, the average estimated covariance remains nearly identical for all three filters 
as shown in Figure 3.11 by the average estimated angle, x\, standard deviation. The mag¬ 
nitude of the average estimated standard deviation is smaller at the higher measurement 
frequency since the filters are receiving information from measurements 100 times more 
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(b) 




(c) (d) 


Figure 3.9. Comparison of EKF, EKF2, UKF estimation performance using a 
physical pendulum process model with accurate 100Hz measurements, 100 
trial average: (a) depicts average absolute angle (xi) error, (b) average 
absolute angular velocity (.* 2 ) error, (c) average absolute l^ob (-* 3 ) error and 
(d) average absolute damping ratio, /?, (* 4 ) error. 


frequently. 

The average innovation shown in Figure 3.12 provides an additional measure of filter per¬ 
formance. A pattern exists in the average innovation plots at both frequencies as a result 
of the model mismatch caused by the damping ratio, X 4 , initialization error. Each esti¬ 
mation algorithm corrects the model by adjusting the damping ratio, resulting in random 
innovations occurring as anticipated in steady-state. 

A representative single trial’s innovations are presented in Figure 3.13 for comparison with 
the experimental results shown later in Figure 3.18 of Section 3.5. 


79 














Moment of Inertia (l Bob ) Error (kgm ) Ang|e E|Tor (rad) 



Time(s) 



Time(s) 

(c) 



-EKF 

-EKF2 

UKF 





V 




°0 10 20 30 40 50 60 

Time(s) 


(b) 


. 6 r x10 ' 3 







-EKF 

-EKF2 



















M 






k, 







Q 0 I- . . . . 

0 10 20 30 40 50 60 

Time(s) 


(d) 


Figure 3.10. Comparison of EKF, EKF2, UKF estimation performance using 
a physical pendulum process model with accurate 1Hz measurements, 100 
trial average: (a) depicts average absolute angle (^i) error, (b) average 
absolute angular velocity (.*2) error, (c) average absolute Igob (*3) error and 
(d) average absolute damping ratio, /J, (. 3 : 4 ) error. 
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Figure 3.11. Comparison of EKF, EKF2 and UKF 100 trial average esti¬ 
mated angle (jci) standard deviation, physical pendulum process model with 
accurate measurements: (a) depicts 100 Hz measurements and (b) 1 Hz 
measurements. 
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Figure 3.12. Comparison of EKF, EKF2 and UKF 100 trial average inno¬ 
vations, physical pendulum process model with accurate measurements: (a) 
depicts 100 Hz measurements and (b) 1 Hz measurements. 
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Figure 3.13. Comparison of EKF, EKF2 and UKF representative single trial 
innovations, physical pendulum process model with accurate measurements: 
(a) depicts 100 Hz measurements and (b) 1 Hz measurements. 
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3.4.2 Poor Measurements 

The results shown in Figures 3.14 and 3.15 are produced using measurements with Gaus¬ 
sian noise, ~ N (0 rad, 1.0 x 10 -4 rad 2 ), representing potentiometer measurement noise. 
They are presented to demonstrate that the frequency effect on filter performance is not a 
result of the measurement noise level, but rather is a direct result of the covariance propa¬ 
gation. The average error is greater than when using more accurate measurements at both 
frequencies due to the lower quality measurement information. Comparison of Figures 3.9 
and 3.14 at 100Hz and Figures 3.10 and 3.15 at 1Hz, respectively, demonstrates the effect 
that measurement quality has on the average error. 




(a) (b) 




(c) (d) 


Figure 3.14. Comparison of EKF, EKF2, UKF estimation performance using 
a physical pendulum process model with poor 100Hz measurements, 100 
trial average: (a) depicts average absolute angle (vq) error, (b) average 
absolute angular velocity (.* 2 ) error, (c) average absolute l^ob (* 3 ) error and 
(d) average absolute damping ratio, /?, (* 4 ) error. 


The average innovation should reflect the measurement noise when the estimator is per- 
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Figure 3.15. Comparison of EKF, EKF2, UKF estimation performance using 
a physical pendulum process model with poor 1Hz measurements, 100 trial 
average: (a) depicts average absolute angle (xi) error, (b) average absolute 
angular velocity (.* 2 ) error, (c) average absolute Igob (* 3 ) error and (d) 
average absolute damping ratio, (3, (* 4 ) error. 


forming as designed in steady state. The average innovations shown in Figure 3.16 demon¬ 
strate that the filters operate as anticipated in steady-state, but as a result of the poor mea¬ 
surement information, steady-state is not obtained at the same time as in Figure 3.12. 

A representative single trial’s innovations are presented in Figure 3.17 for comparison with 
the experimental results shown later in Figure 3.23. 

The simple pendulum simulation reveals that estimator performance is not impacted despite 
using a measurement frequency, 1 Hz, approximately equivalent the pendulum’s period, 0.8 
Hz. These results do not preclude EKF sensitivity to lower measurement rates, such as 0.1 
Hz, that have measurement intervals spanning multiple periods. 
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Figure 3.16. Comparison of EKF, EKF2 and UKF 100 trial average in¬ 
novations, physical pendulum process model with poor measurements: (a) 
depicts 100 Hz measurements and (b) 1 Hz measurements. 
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Figure 3.17. Comparison of EKF, EKF2 and UKF representative single trial 
innovations, physical pendulum process model with poor measurements: (a) 
depicts 100 Hz measurements and (b) 1 Hz measurements. 
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3.5 Experimental Results 

A single trial of the pendulum experiment is conducted using the data collection system 
described in Section 3.2. The experiment is performed to verify that the EKF, EKF2, and 
UKF respond to varied measurement frequency in the same manner in practical applica¬ 
tion and simulation. An assumption of Gaussian noise is made despite the encoder and 
potentiometer displaying quantization noise in Figure 3.5. 

The measurement interval is irregular since the data is collected at the fastest possible rate 
for the system. Results presented as 100 Hz are sampled at an average frequency of 100 Hz, 
but include data sampled as slow as 90 Hz and as fast as 189 Hz. Results presented as 1 Hz 
are sampled at an average frequency of 1.002 Hz, but include data sampled as slow as 
0.999 Hz and as fast as 1.068 Hz. 

3.5.1 Accurate Measurements 

Figure 3.18 show bias in the innovations indicative of the challenges associated with ap¬ 
plying a filter without process noise in application. Despite the process model mismatch 
that is visible in the form of the structured innovations, all three estimators converge after 
approximately t - 5 s at the 100 Hz measurement frequency. Figure 3.19 shows I Bob, * 3 , 
and damping coefficient, X4, attain a steady-state result around which their values oscillate 
after this time. All estimators produce nearly identical results; only the UKF, which is 
plotted last, is visible. Figure 3.20 shows that all estimators also converge in I Bob, * 3 , and 
damping coefficient, X 4 after approximately t = 5 s at the 1 Hz measurement frequency. 
Interestingly, the damping coefficient is estimated to be negative in the dynamic response 
when using the 100 Hz measurement frequency. A negative damping coefficient has the 
effect of adding energy into the pendulum system contrary to the experiment set-up. All 
estimators maintain a positive damping coefficient when using the 1 Hz measurement fre¬ 
quency. 

The estimators produce nearly identical results at both measurement frequencies as clearly 
seen in Figures 3.21b and 3.22b. The experiment confirms the simulation results presented 
in Figures 3.9 and 3.10. Measurement frequency does not impact the EKF performance at 
lower measurement frequencies as seen in the falling body problem. 
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Figure 3.18. Comparison of EKF, EKF2 and UKF innovations, pendulum 
experiment single trial, with accurate measurements: (a) depicts 100 Hz 
measurements and (b) 1 Hz measurements. 
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Figure 3.19. Comparison of EKF, EKF2 and UKF performance, pendulum 
experiment single trial, with 100 Hz accurate measurements: (a) depicts 
hob (* 3 ) and (b) damping ratio, /? (* 4 ). 
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Figure 3.20. Comparison of EKF, EKF2 and UKF performance, pendulum 
experiment single trial, with 1 Hz accurate measurements: (a) depicts I Bob 
(X 3 ) and (b) damping ratio, (*4). 
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Figure 3.21. Comparison of EKF, EKF2 and UKF performance, pendulum 
experiment single trial, with 100 Hz accurate measurements: (a) depicts 
angle (.* 1 ) and (b) angle (xi), expanded view. 
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Figure 3.22. Comparison of EKF, EKF2 and UKF performance, pendulum 
experiment single trial, with 1 Hz accurate measurements: (a) depicts angle 
(.¥ 1 ) and (b) angle (jcj), expanded view. 
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3.5.2 Poor Measurements 

Estimator convergence properties are most clearly revealed through innovations shown in 
Figure 3.23. The innovations are biased at both frequencies. The effects of process model 
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Figure 3.23. Comparison of EKF, EKF2 and UKF innovations, pendulum 
experiment single trial, with poor measurements: (a) depicts 100 Hz mea¬ 
surements and (b) 1 Hz measurements. 


and measurement model errors result in the innovations failing to display Gaussian noise 
characteristics. 

Figure 3.24 shows that all estimators reach steady-state in I Bob, *3, and damping coefficient, 
X 4 after approximately t = 15 s at the measurement 100 Hz frequency. Close inspection 
of Figure 3.24 reveals that the measurement model error, the potentiometer hysteresis, pre¬ 
vents the full convergence that exists in simulation. All estimators produce nearly identical 
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(a) 



Figure 3.24. Comparison of EKF, EKF2 and UKF performance, pendulum 
experiment single trial, with 100 Hz poor measurements: (a) depicts l^ob 
(* 3 ) and (b) damping ratio, (3 (*4). 


results; only the UKF, which is plotted last, is visible. Figure 3.25 shows that all estimators 
fail to converge in I Bob, * 3 , and damping coefficient, X 4 prior to t = 40 s at the 1 Hz mea¬ 
surement frequency. The initial dynamic response is less than half the magnitude of the 
100 Hz measurement frequency. All estimators maintain a positive damping coefficient at 
both measurement frequencies in contrast to the use of encoder measurements at 100 Hz. 

The estimators produce nearly identical results as can be clearly seen in Figures 3.26b and 
3.27b at both measurement frequencies. The experiment confirms the simulation results 
presented in Figures 3.14 and 3.15. Measurement frequency does not impact the EKF 
performance at lower 1 Hz measurement frequencies as seen in the falling body problem. 
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Figure 3.25. Comparison of EKF, EKF2 and UKF performance, pendulum 
experiment single trial, with 1 Hz poor measurements: (a) depicts l^ob (* 3 ) 
and (b) damping ratio, (* 4 ). 
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Figure 3.26. Comparison of EKF, EKF2 and UKF performance, pendulum 
experiment single trial, with 100 Hz poor measurements: (a) depicts angle 
(;ti) and (b) angle (jci), expanded view. 
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Figure 3.27. Comparison of EKF, EKF2 and UKF performance, pendulum 
experiment single trial, with 1 Hz poor measurements: (a) depicts angle 
(.¥i) and (b) angle (*i), expanded view. 


98 








3.6 Conclusions 

The experimental results of Section 3.5 validate the results obtained through simulation 
in Section 3.4. Although only a single experimental trial is presented in this dissertation, 
multiple experimental trials were conducted that produced near identical EKF and UKF es¬ 
timator performance. The experimental confirmation of the simulation provides confidence 
that the results of a sufficiently large Monte Carlo study of another problem should produce 
results that would be experimentally confirmed. 

Additionally, the simple pendulum problem studied in this chapter reveals the effect of 
measurement frequency on EKF performance is not visible in all nonlinear estimation prob¬ 
lems. While the UKF demonstrated superior performance at low measurement frequencies 
in comparison to the EKF for the single-dimension falling body problem of Chapter 2, both 
the EKF and UKF displayed nearly identical performance when using relatively sparse 
measurements in estimating pendulum parameters. This result, when viewed in conjunc¬ 
tion with the Chapter 2 results, helps to explain the conflicting views regarding KF-based 
nonlinear estimator performance present in the literature. 

Chapter 4 seeks to provide tools to help determine whether measurement frequency will im¬ 
pact EKF or UKF performance. The single-dimension falling body and pendulum problems 
provide examples to develop concepts for determining sensitivity to sparse measurements 
in other nonlinear estimation applications. 
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CHAPTER 4: 

Predicting Estimator Sensitivity to Sparse Tempora 

Measurements 


The measurement frequency sensitivity present in the falling body problem (Chapter 2) is 
not seen in the pendulum problem (Chapter 3). The EKF and UKF estimate the a priori 
covariance using different approximations that may affect estimator performance in some 
applications. This chapter presents an approach to predict the sensitivity of KF-based 
nonlinear estimators to sparse temporal measurements. 

The primary difference between the UKF and EKF is the approximation used to allow 
application of the KF structure. As noted in Chapter 1, an approximation is necessary to 
apply the KF structure to a problem with a nonlinear process or measurement model. The 
UT approximates the estimated state’s PDF whereas the EKF approximates the nonlinear 
transformation. Section 4.1 considers analysis of covariance propagation using the different 
approximations to determine estimator sensitivity to measurement frequency. 

Analysis of the covariance propagation over an entire trajectory is challenging and would 
provide little value in problems that are highly nonlinear over only limited portions of 
the state trajectory (i.e., the falling body problem considered in Chapter 2). Therefore, Sec¬ 
tion 4.2 investigates the impact of the EKF approximation through consideration of the pro¬ 
cess model linearization that occurs in the EKF. The Jacobian and covariance propagation 
analysis techniques are applied to the falling body and pendulum problems in Section 4.3. 
Conclusions are provided in Section 4.4. 

4.1 Covariance Propagation Analysis 

The KF structure produces a state estimate that is a linear combination of a predicted state 
estimate and the scaled difference of the actual and predicted measurements [3]. The scal¬ 
ing factor, or Kalman gain, is determined from the a priori, or propagated, covariance, as 
shown in Equation (1.12). Asa result, the information available in the measurement is only 
properly incorporated if the a priori covariance is computed accurately. The measurement 
information is not properly used if the covariance is underestimated. In this case, the esti- 
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mator “trusts” the prediction and rejects the measurement information update. Maybeck [8] 
notes that underestimating covariance leads to filter divergence. In instances when the co- 
variance is overestimated, the filter “trusts” the measurement more, and the predicted state 
is unnecessarily updated and filter effectiveness is reduced. 

The response of the nonlinear KF-based estimators is similarly responsive to covariance 
estimation errors. Thus, propagation of an arbitrary covariance matrix through a specified 
nonlinear transformation over a varied time interval provides insight into each filter’s per¬ 
formance. Propagation by the Monte Carlo technique using a sufficiently large number 
of trials provides an asymptotically close approximation of the true propagated covariance 
matrix. This result can be compared with propagation performed using the first-order Tay¬ 
lor series linearization (EKF) and the UT (UKF) to determine predicted filter performance 
over different measurement intervals. 

There are many different approaches to compare the EKF and UKF covariance matrix prop¬ 
agation with the Monte Carlo propagation. Similarity of the EKF and UKF covariance ma¬ 
trices, respectively, with the Monte Carlo propagation provides one useful measure. The 
JBLD divergence value, detailed in Equation (2.18), is used as a measure of covariance 
matrix similarity. 

Additionally, a comparison of each term along the main diagonal of the covariance matri¬ 
ces can provide insight. This comparison is used to gain a sense of whether the covariance 
associated with each state is correctly estimated, underestimated, or overestimated. Rel¬ 
ative estimation performance is useful to predict whether the filter will be unnecessarily 
rejecting or improperly incorporating measurement information. 

The JBLD divergence value similarity comparison and the main diagonal analysis ap¬ 
proaches are applied to the falling body problem in Section 4.3.1 and the pendulum problem 
in Section 4.3.2. 


4.2 Jacobian Analysis 

The EKF uses the state transition matrix to propagate the estimated covariance as shown 
in Equation (1.32). The state transition matrix shown in Equation (4.1) [5] is integrated to 
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produce a state transition matrix that spans the integration interval. 


d®t k +\/k 

dt 


= O 


(4.1) 


where, O = j . The linearization point in the EKF is the a posteriori state esti¬ 

mate. Therefore, the distance from the linearization point at the time of the next measure¬ 
ment increases as the measurement interval increases. An integration interval shorter than 
the measurement interval can be employed to reduce linearization error by dividing the 
measurement interval propagation into shorter integration steps [20]. The state transition 
matrix is assumed to be the identity matrix at the start of the integration interval. 13 


While relatively small integration time steps will reduce the numerical linearization error, 
systems may experience a rapid change in the Jacobian such that linearization error from 
the EKF algorithm becomes apparent when the measurement interval is relatively long. 
The process model Jacobian, the partial derivative of the dynamic model, provides insight 
as to the effect that measurement frequency will have in a given problem. 


The Jacobian is multi-dimensional and time varying in most practical applications prohibit¬ 
ing determination of the linearization error through inspection. A time varying metric is 
needed to locate regions along the trajectory where EKF performance may be degraded 
over longer measurement intervals. The Frobenius norm is one such metric that is suitable 
for this application. The Frobenius norm of the Jacobian reduces the required analysis of 
the multi-dimensional matrix to consideration of a time-varying scalar. The time varying 
Frobenius norm can further be differentiated to provide insight as to the Jacobian rate of 
change along the state trajectory. 

This approach is applied to the falling body problem in Section 4.3.1 and the pendulum 
problem in Section 4.3.2 to demonstrate that the Frobenius norm of the Jacobian is useful 
in determining regions of the state trajectory that require additional inspection using the 
techniques of Section 4.1. 

13 The initial state transition matrix is the identity matrix because the state does not change spontaneously. 
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4.3 Analysis Application Examples 

The concepts presented in Sections 4.1 and 4.2 are applied to two problems, the single¬ 
dimension falling body of Chapter 2 and the simple pendulum of Chapter 3, to demonstrate 
their value in helping to predict the KF-based nonlinear estimator sensitivity to sparse 
temporal measurements. 


4.3.1 Falling Body Problem 

Equation (4.2) shows the process model Jacobian for the falling body problem. 


F(x,t) 


df(x,t) 

dx 


0 -1 

y exp(-y.vi)jc^.x:3 -2exp(-y.vi)jC2^3 
0 0 

0 0 


0 

- exp(-yx | )*2 
0 
0 


0 

1 

0 

0 


(4.2) 


The Frobenius norm of the Jacobian is shown in Equation (4.3). This value provides a 
metric to study the change in the Jacobian with respect to time. High rates of change 
in the Jacobian indicate regions of the state trajectory that may be subject to increased 
linearization error, as discussed in Section 4.2. 

l|F(jc,f)||/r = Jexp(-l x 1(T 4 .*i) (x 4 2 + 4(jt 2 *3) 2 + 2.5 x \0r 9 (x\x\j) + 2 (4.3) 

Figure 4.1 shows the Frobenius norm of the Jacobian evaluated along the simulation truth 
trajectory. The rate of change of the Frobenius norm of the Jacobian provides some insight 
into the impact of measurement interval on the propagation of covariance. Figure 4.1b 
shows the change in the Jacobian with respect to time for the truth model. The rather large 
magnitude change in the Frobenius norm of the Jacobian can translate into error in the 
estimated covariance. This effect may be addressed by reducing the integration time step 
during the measurement interval to avoid significant numerical linearization errors, but it 
cannot overcome the error resulting from the state being a large distance away from the 
linearization point in this portion of the trajectory. 

This analysis technique fails to provide insight as to the specific state that will be most 
impacted, but points to a significant challenge to the EKF’s underlying assumptions. The 
UKF is not impacted by this specific approximation error. However, it is susceptible to 
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Figure 4.1. Falling body process model Jacobian Frobenius norm, along 
simulation truth trajectory: (a) depicts the process model Jacobian Frobenius 
norm and (b) the derivative of the process model Jacobian Frobenius norm. 


mismatches between the actual state PDF versus the PDF represented by the sigma points. 
Higher-order moment mismatch is more visible in the covariance estimate than the state 
estimate [19]. 

Additional insight is obtained by propagating the state for both 0.5 and 2 second intervals 
with an arbitrarily assumed covariance starting at different points in the state trajectory. 
Propagation is performed using the Monte Carlo technique with 10,000 random points to 
generate an approximation of the true covariance propagation. This value is compared with 
covariance propagation via the state transition matrix used in the EKF and the unscented 
transform used in the UKF. 
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The covariance shown in Equation (4.4) is propagated for a 2 second interval assuming the 
state value at 8 seconds, 10 seconds, and 12 seconds. 


Po 


1 x 10 6 0 0 0 

0 1 x 10 6 0 0 

0 0 2.5 x 10“ 9 0 

0 0 0 1 x 10“ 4 


(4.4) 


Covariance propagation commencing at 0 seconds is provided for comparison as the results 
shown in Section 2.3.2 reflect similar initial EKF and UKF estimation performance in spite 
of the longer measurement interval. Results from each starting time are shown in Table 4.1. 
The state transition and UT approaches generate the identical JBLD divergence value for 


Table 4.1. JBLD divergence value comparison of the state transition ma¬ 
trix and UT propagated covariance similarity with Monte Carlo propagated 
covariance, falling body problem, 2 second propagation. 


to 

0 (s) 

8 (s) 

10 (s) 

12 (s) 

State Transition 

1.6118 x HU 2 

1.6360 x HU 2 

1.5734 x KU 2 

3.1684 x Hr 2 

UT 

1.6118 x 10“ 2 

1.5745 x 10“ 2 

1.1009 x 10“ 2 

2.0880 x 10- 2 


the 0 second starting time. The UT produces a covariance matrix that is more similar to the 
Monte Carlo at the 8, 10, and 12 second starting points. The difference between the state 
transition matrix and UT divergence values is largest at the 12 second starting point. 

The percent of covariance propagation error between the state transition and UT techniques 
and the Monte Carlo propagation is shown in Figure 4.2. The covariance of each state is 
represented by the respective covariance matrix main diagonal term. 

These results demonstrate that propagation of covariance internal to the EKF predictor 
step underestimates the velocity covariance by approximately 2.5% for the time interval 
from 10 to 12 seconds. While additional factors also effect EKF performance, this is the 
root cause for the estimator being unable to take proper advantage of further measurement 
information. The UKF also underestimates the velocity covariance by approximately 0.6%, 
but it continues to function. 
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Figure 4.2. Arbitrary 2 sec covariance propagation from the specified falling 
body truth trajectory starting point: (a) depicts altitude, x\, covariance 
% error, (b) velocity, X 2 , covariance % error, (c) ballistic coefficient, x$, 
covariance % error, and (d) gravitational acceleration, x\, covariance % 
error. 


Figure 4.2 also shows that the propagation of covariance internal to the UKF predictor step 
overestimates the altitude covariance by approximately 2.3% and the velocity covariance 
by approximately 2.7% for the time interval from 12 to 14 seconds. This overestimation 
results in the large altitude estimation error seen in Figure 2.9. This analysis does not 
account for a potential PDF mismatch in that the initial PDF is assumed to be Gaussian for 
each propagation. 

Covariance propagation for 0.5 seconds, the propagation interval equivalent with the 2 HZ 
measurement frequency, is shown in Figure 4.3. The EKF and UKF propagation approx- 


107 



































imation techniques produce covariance results nearly identical to one another with the 
magnitude of the greatest difference less than 0.01%. This explains the nearly identical 
performance between the methods as seen in Figure 2.6. 
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Figure 4.3. Arbitrary 0.5 sec covariance propagation from the specified falling 
body truth trajectory starting point: (a) depicts altitude, x\, covariance 
% error, (b) velocity, X 2 , covariance % error, (c) ballistic coefficient, X 3 , 
covariance % error, and (d) gravitational acceleration, * 4 , covariance 
error. 


% 


JBLD divergence values are shown in Table 4.2. These values demonstrate that the propa¬ 
gated covariance matrices are nearly identical starting at the four different points along the 
state trajectory at the 2 Hz measurement frequency. The similar accuracy of the propagated 
covariance is reflected in similar performance achieved by the EKF and UKF using 2 Hz 
measurements. 
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Table 4.2. JBLD divergence value comparison of the state transition ma¬ 
trix and UT propagated covariance similarity with Monte Carlo propagated 
covariance, falling body problem, 0.5 second propagation. 


6) 

0 (s) 

8 (s) 

10 (s) 

12 (s) 

State Transition 

1.9468 x HU 2 

1.8545 x lCT 2 

1.5307 x lCU 2 

1.2719 x 10“2 

UT 

1.9468 x 10“ 2 

1.8547 x 10“ 2 

1.5318 x IQ’ 2 

1.2728 x IQ” 2 


4.3.2 Pendulum Problem 

Sparse temporal measurements do not produce a similar effect on the EKF steady-state 
performance in the pendulum problem as seen in Section 2.3. However, analysis of this 
problem helps to predict when a measurement frequency induced difference in steady-state 
estimation error would not occur in other problems. 

Equation (4.5) shows the process model Jacobian for the pendulum problem. 

0 1 0 0 " 

MLg cos(xi) —X4 MLg s,m(x\)+X2X4 —xi 

X3+I X3+I (X 3 +/) 2 X3+I M 

0 0 0 0 

0 0 0 0 


F(x,t) = 


df(x,t ) 
dx 


The Frobenius norm of the Jacobian, shown in Equation (4.6), is considered for comparison 
with the falling body problem. 


II^,0IIf = 


1 


O3+0.0159) 2 


X 2 + x% + 


0.1982 (cos(.n)) 2 ) (x 3 + 0.0159)- 


+ 


(0.4452 sin(xi) + X2X4) 2 + (*3 + 0.0159) z 


(4.6) 


Figure 4.4 shows the Frobenius norm of the Jacobian evaluated along the simulation truth 
trajectory. The rate of change of the Frobenius norm of the Jacobian is shown in Fig¬ 
ure 4.4b. The magnitude and rate of change decrease as the magnitude of the pendulum 
oscillation is damped. The largest rate of change in the Jacobian occurs at the start of the 
trajectory, in contrast to the falling body problem, as shown in Figure 4.4b. 

An arbitrary covariance matrix, Equation (4.7), is propagated forward in a similar fashion 
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20 30 40 

Time (s) 

(b) 


Figure 4.4. Pendulum process model Jacobian Frobenius norm, along sim¬ 
ulation truth trajectory: (a) depicts the process model Jacobian Frobenius 
norm and (b) the derivative of the process model Jacobian Frobenius norm. 


to the analysis performed in Section 4.3.1. 


1 x 10“ 4 0 0 

0 1 x 10“ 4 0 

0 0 1 x 10“ 8 

0 0 0 


0 

0 

0 

1 x 10“ 6 


(4.7) 


The state transition and UT propagated covariance matrices for propagation intervals of 
0.01, 1, 2, and 10 seconds are compared with covariance matrices propagated using a 
10,000 point Monte Carlo. All propagation commences at t = 0 seconds, the point in 
the state trajectory corresponding with the largest rate of change of the Jacobian. JBLD di- 
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vergence values are shown in Table 4.3. These divergence values characterize the effect the 


Table 4.3. JBLD divergence value comparison of the state transition ma¬ 
trix and UT propagated covariance similarity with Monte Carlo propagated 
covariance, pendulum problem, varied propagation interval. 


Propagation Interval 

0.01 (s) 

1 (s) 

2 (s) 

10 (s) 

State transition 

1.5413 x HU 2 

1.5679 x HU 2 

1.7018 x HU 2 

8.4330 x 10” 1 

UT 

1.5410 x 10“ 2 

1.4887 x 10“ 2 

1.4348 x 10“ 2 

1.5430 x 10“ 2 


different propagation approaches have on the covariance matrices. Propagation intervals 
representative of 100 Hz and 1 Hz demonstrate small differences between the state tran¬ 
sition and UT covariance propagation. However, the UT covariance propagation remains 
substantially more like the Monte Carlo propagated covariance as the propagation interval 
lengthens in excess of one pendulum period. 

As discussed in Section 4.3.1, the JBLD divergence value information is complemented 
by the comparison of propagated covariance through the % error of the main diagonal 
terms. These values are shown in Figure 4.5 and stand in contrast to the falling body 
problem comparison shown in Figure 4.2. The covariance propagation is nearly identical 
over the intervals associated with the measurement frequencies presented. Exceptionally 
long measurement time intervals, on the order of 10 seconds, are necessary to demonstrate 
the difference in propagation approaches. 
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Figure 4.5. Comparison of covariance propagation from x(t = 0): (a) depicts 
angle, jci, covariance % error, (b) angular velocity, X 2 , covariance % error, 
(c) Isob, * 3 , covariance % error, and (d) damping coefficient, X 4 , covariance 
% error. 


4.4 Conclusions 

This chapter presents an analysis approach to determine if sparse temporal measurements 
are expected to impact KF-based nonlinear estimator performance. The nonlinearity of 
the process model can be characterized through the rate of change of the Frobenius norm 
of the Jacobian evaluated along the state trajectory. The Frobenius norm of the Jacobian is 
used to provide a scalar metric for comparison. This approach highlights regions of interest 
where a linearized approximation of the nonlinear transformation may result in significant 
approximation error. 

Insight into estimator performance can be gained through propagation of an arbitrary co- 
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variance over the measurement interval. Details of this approach are discussed in Sec¬ 
tion 4.1. Comparison of covariance propagation using the state transition matrix and the 
UT, respectively, with a Monte Carlo propagation reveals the quality of the covariance es¬ 
timate. The single-dimension falling body problem and the simple pendulum are analyzed 
using the proposed approach. The analysis method confirms that the falling body EKF 
estimator is susceptible to deteriorating performance as the measurement interval length¬ 
ens. The EKF estimator for the pendulum problem is shown not impacted by measurement 
intervals considered in Chapter 3. 
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CHAPTER 5: 

Conclusions and Future Work 


5.1 Conclusions 

This dissertation investigates the effect of measurement frequency on KF-based nonlinear 
estimators. The EKF and UKF employ fundamentally different approximation techniques 
to the challenging nonlinear estimation problem. Details of each filter’s construction are 
presented in Chapter 1. The KF requires propagation of both the state’s mean and co- 
variance between measurement intervals. Propagation is necessary to generate a predicted 
state, as the mean and covariance at the measurement time are required to describe the 
state. The predicted state is corrected through a weighted linear combination of the differ¬ 
ence between the measurement and the predicted measurement. 

Julier and Uhlmann [11] note that the UKF’s approximation technique is more accurate than 
the first order Taylor series linearization used by the EKF. This view leads to an expecta¬ 
tion that the UKF should outperform the EKF in steady-state in instances with a lengthy 
measurement interval. Conversely, one would expect that the filters should perform simi¬ 
larly over sufficiently short measurement intervals. The same measurement frequency, and 
its associated propagation interval, may be sufficiently fast in one problem while relatively 
slow in another. 

This hypothesis is validated through the use of two example problems, the single dimension 
falling body of Chapter 2 and a simple pendulum investigated in Chapter 3. The measure¬ 
ment frequency is shown to significantly alter the relative performance of the EKF and 
UKF in the first problem, but has minimal impact in the second. Further analysis is pre¬ 
sented in Chapter 4 demonstrating use of the Frobenius norm of the Jacobian matrix along 
the state trajectory to characterize system nonlinearity. Characterization allows for targeted 
investigation of covariance propagation with different approximation techniques. 

Covariance overestimation leads to poor estimator performance, but does not lead to com¬ 
plete estimator failure. However, filter divergence occurs when the covariance is underesti¬ 
mated and must be avoided. Covariance propagation interval length can greatly impact both 
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the relative quality and direction of estimation error. This error can be described through 
use of a similarity comparison between the covariance matrices generated by different ap¬ 
proximation approaches and a Monte Carlo propagated covariance matrix. This analysis 
provides a sense of the propagation quality. Determination of the direction of estimation 
error, either under or over, is shown through comparison of propagated covariance matrices 
main diagonal components. 

Lengthened measurement intervals reveal undocumented challenges with UKF implemen¬ 
tation. All sigma point vectors components should satisfy constraints to ensure valid prop¬ 
agation of the state estimate. Chapter 2 demonstrates that initialization can lead to sigma 
point vectors that violate constraints. While the UKF may still function, it can produce un¬ 
predictable results depending upon the specific system dynamics. This aspect should alter 
the manner in which a UKF is initialized. 

Chapter 2 presents a novel approach to constraining sigma points. The scaled UT, initially 
created to minimize non local effects and match higher-order moments, is demonstrated 
to improve the UKF’s robustness by altering the sigma point vectors without changing 
the represented mean and covariance. Application of the scaled UT for the purpose of 
respecting state constraints requires the mean to be located in the allowable region. This 
can be achieved in the UKF construct through enforcement of the constraints on the a 
posteriori state estimate. The UKF-S presented in Chapter 2 uses adaptive scaling of the 
Kalman gain to ensure that the a posteriori state estimate respects constraints. Kalman gain 
scaling and the scaled UT are applied only as necessary to respect parameter constraints. 

5.2 Future Work 

The addition of process noise can be used to mask the effect of measurement frequency. 
This can be particularly effective approach to improve estimator robustness at the expense 
of estimation accuracy. Application of an UKF-S without the use of process noise may 
demonstrate robust performance without the expense of reducing estimator accuracy. 

As noted, UKF-S performance may be improved by altering the size of the minimum hyper¬ 
sphere radius along the state trajectory. The effect of sigma points located in close proxim¬ 
ity to the mean is significantly reduced in regions that are approximately linear. The results 
presented in Chapter 2 suggest that the guard band size may be altered to limit the negative 
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effect of the smaller hyper-sphere radius in the more nonlinear region of the trajectory. 

Further work is needed to verify the applicability of the presented measurement frequency 
analysis on more complex dynamic systems such as those implemented in inertial naviga¬ 
tion systems. Additionally, high-order moment-matched sigma points, such as the CUT or 
FIS points briefly mentioned in Chapter 1 may improve covariance propagation in nonlinear 
transformations in which the higher-order moments have a greater impact. A constrained 
UKF may benefit from representing process and measurement noise with a bounded vice 
Gaussian PDF. 
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